14浏览
查看: 14|回复: 2

[项目] 【Arduino 动手做】YABR:一款使用步进电机的自平衡机器人

[复制链接]
您的 Arduino 平衡机器人 (YABR) 是一款自平衡机器人,您可以将其构建为学校项目或与孩子一起构建的有趣项目。它可能看起来很简单,但您可以从构建这个自平衡机器人中学到很多东西。

与大多数自平衡机器人相比,这款机器人使用步进电机而不是常规直流电机。主要原因是步进电机精密,在电池电压下降时没有性能损失。一个脉冲始终是精确的运动量。普通直流电机可能存在机械摩擦和电阻差异。这可能会导致性能差异。因此,机器人不会沿直线移动。

如果您使用下面的硬件列表,构建此机器人的总成本约为 80 美元。这包括电池、双节棍、充电器、步进电机等。

您可以免费下载的 Arduino 程序是 100% 自编写的,不基于任何其他软件。该代码注释良好,解释清晰。这样就可以根据自己的目的进一步开发代码。

如果您在构建或设置过程中遇到任何问题,请先查看 Q&A 页面。大多数问题已经得到了详细的回答。

第 1 步 - 软件
首先下载完整的 YMFC-AL 软件包:

YABR.zip(版本 1.1)

第 2 步 - 硬件
要构建此机器人,您需要硬件。我制作以下列表只是为了方便起见。您可以从不同来源自由获取自己的硬件。但这是我使用/订购的硬件:

1 个 Arduino pro 迷你克隆
1 x FTDI USB 转 TTL 编程器,用于 Arduino pro mini
1 个 Arduino Uno 克隆
1 个 MPU-6050 陀螺仪和加速度计
2 个 2.4G 无线串行收发器模块
2 x 35mm 步进电机
2 x Geeetech StepStick DRV8825
1 x Wii 有线双节棍控制器
1 x Mini DC 7~28V 转 DC 5V 降压转换器
1 x 11.1V 2200mAh 30C 锂聚合物电池
1 个 B3AC 2S/3S 锂电池平衡充电器
如果 35mm 步进电机缺货。您也可以使用这些步进电机。请注意,这些是 42 毫米而不是 35 毫米。所以你必须修改框架。

2 x 42mm 步进电机
以下大部分零件都可以在您当地的电子商店找到。但如果你附近没有电子产品商店,我会在这里放链接:

1 x 1/4W 陶瓷金属膜电阻器套件 (600 PCS)
1 x 玻璃纤维原型 PCB
1 x 3 针拨动开关(5 件装)
最后,您需要一个旧内胎和两张胶合板。我使用了 2.5 毫米和 12.5 毫米的板材。

第 3 步 - 工具
当然,您需要一些简单的工具,例如烙铁、螺丝刀、品格锯、紧凑型钻头等。

第 4 步 - 构建
观看 YABR 硬件构建视频,并根据完整软件包中包含的视频和原理图构建机器人。

        构建 YABR 平衡机器人。
我自己的 YABR 平衡机器人的详细图片可以在本项目页面的媒体部分找到。


4.1 二极管和电阻器
点击查看完整图片
原理图上的电阻器 R1 是将程序上传到 Arduino 所必需的。收发器的 TXD 输出被强制为高电平或低电平。因此,FTDI 程序员无法再更改此输出,您将收到上传错误。通过添加此电阻器,FTDI 程序员可以更改 Arduino 的 RX 引脚上的电压,而不管收发器输出和程序的状态如何,都没有任何问题。

其他两个电阻(R2 和 R3)形成一个分压器。这意味着电池的 12.6 伏减去二极管上的 0.6 伏电压降除以 2.5。当电池充满电时,模拟输入上会产生 4.8 伏特的电压。在主程序中,此模拟输入将用于保护电池。这是因为当每节电池的电压降至 3 伏以下时,锂电池可能会损坏。

二极管 D1 保护所有电子元件免受反极性的影响。因此,当您不小心反转电池的连接时,组件不会冒烟。


4.2 MPU-6050 陀螺仪/加速度计
点击查看完整图片
YABR 软件唯一支持的陀螺仪/加速度计是 MPU-6050。这是因为自调平功能需要一个加速度计和一个陀螺仪,正如我在这两个视频中所解释的那样:

        MPU-6050 用于自动调平四轴飞行器的 6dof IMU 教程 - 第 1 部分
        MPU-6050 用于自动调平四轴飞行器的 6dof IMU 教程 - 第 2 部分
陀螺仪的方向很重要。确保以与此图所示完全相同的方向安装陀螺仪。否则 YABR 软件无法计算出正确的角度,机器人将无法工作。

4.3 硬件测试
请注意,您正在焊接 PCB 背面的电线。原理图从前面面向元件绘制。所以一切都是镜像的,在将任何电源连接到 PCB 之前,您需要仔细检查所有连接。

一切就绪后,是时候将 FTDI 编程器连接到 Arduino pro mini 了。暂时不要连接电池。如果 LED 不亮,则表示接线短路,您需要尽快断开 FTDI 编程器。通常,Arduino pro mini 已经使用闪烁草图进行了编程,因此 Arduino 上的 LED 应该开始闪烁。

要检查陀螺仪是否正确连接并检查机器人的平衡点,您需要上传“hardware-check”程序,该程序可以在之前下载的软件包中找到。

测试机器人的平衡角度并将其固定在支架上的该位置,如我在视频中所示。上传 “hardware-check” 程序,打开串口监视器并将波特率设置为 9600。

上传程序后,打开串口监视器,将波特率设置为 9600。

该程序将检查是否连接了任何 I2C 设备,以及这是否是 MPU-6050。如果一切按预期工作,程序将在屏幕上输出多个原始陀螺仪值。这些只是示例,您的值可能有所不同。记下您在串行输出中看到的余额值。稍后在主程序中将需要它。

4.4 限制电机电流
待办事项列表中的下一件事是将步进控制器设置为正确的驱动电流。如果电机电流设置为高,步进控制器会发热,并且可能会损坏。

首先将电位器设置在下图所示的相同位置。现在,首次连接 lipo 电池时请务必小心。短路会导致大电流、发热、火花和灼伤。

一个好的选择是使用如下所示的小型直流保险丝。如果 PCB 背面的接线短路,该保险丝会熔断。

设置正确电流并检查接线是否正确连接的最简单、最安全的方法是使用台式电源测量电源线中的电流。电源限制为 500 毫安培。这是一项额外的安全功能。如果发生短路,电源会将电流限制为 500 毫安,接线会很好。

首先连接一个电机。使用电位计,可以将电流设置在 100 到 150mA 之间。这足以获得良好的性能。对另一个步进控制器执行相同的作。

如果您无法测量电流,只需将电位计设置在此位置,然后感觉步进驱动器是否不发热。但测量电流总是最好的。

3.5 遥控器
如果你打开 Nunchuck,你可以注意到连接到各种引脚的电线颜色,正如你在原理图上看到的那样。Nunchuck 在 3.3V 下工作,您可以使用 Arduino Uno 的 3.3V 输出为 Nunchuck 供电。

5V 输出可用于收发器。同样,按照原理图上显示的方式连接电线以使其工作。

要检查 Nunchuck 是否正确连接,您需要上传“hardware-check”程序,您可以在之前下载的软件包中找到该程序。上传程序后,打开串口监视器,将波特率设置为 9600。

该程序将检查是否连接了任何 I2C 设备,以及这是否是双节棍。如果一切按预期工作,程序将在屏幕上输出多个原始摇杆值。

第 5 步 - 上传软件
最后,您可以将 YABR-remote 程序上传到 Arduino Uno,将 YABR-robot 程序上传到 Arduino pro mini。

YABR-remote 程序不需要任何修改。在机器人程序中,您需要将加速度计校准值更改为硬件测试程序在机器人处于平衡位置时显示的值。

要启动机器人,您需要打开遥控器的电源。将机器人放在它的背上并打开电源。LED 将闪烁,表示陀螺仪正在校准。当闪烁停止时,您可以慢慢抬起机器人,它将自动开始自行平衡。

基本上就是这样。您现在可以使用 Nunchuck 控制机器人。

【Arduino 动手做】YABR:一款使用步进电机的自平衡机器人图1

【Arduino 动手做】YABR:一款使用步进电机的自平衡机器人图2

【Arduino 动手做】YABR:一款使用步进电机的自平衡机器人图3

【Arduino 动手做】YABR:一款使用步进电机的自平衡机器人图4

驴友花雕  中级技神
 楼主|

发表于 4 小时前

【Arduino 动手做】YABR:一款使用步进电机的自平衡机器人

项目代码

  1. ///////////////////////////////////////////////////////////////////////////////////////
  2. //Terms of use
  3. ///////////////////////////////////////////////////////////////////////////////////////
  4. //THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
  5. //IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
  6. //FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
  7. //AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
  8. //LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
  9. //OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
  10. //THE SOFTWARE.
  11. ///////////////////////////////////////////////////////////////////////////////////////
  12. #include <Wire.h>                                            //Include the Wire.h library so we can communicate with the gyro
  13. int gyro_address = 0x68;                                     //MPU-6050 I2C address (0x68 or 0x69)
  14. int acc_calibration_value = 1000;                            //Enter the accelerometer calibration value
  15. //Various settings
  16. float pid_p_gain = 15;                                       //Gain setting for the P-controller (15)
  17. float pid_i_gain = 1.5;                                      //Gain setting for the I-controller (1.5)
  18. float pid_d_gain = 30;                                       //Gain setting for the D-controller (30)
  19. float turning_speed = 30;                                    //Turning speed (20)
  20. float max_target_speed = 150;                                //Max target speed (100)
  21. ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  22. //Declaring global variables
  23. ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  24. byte start, received_byte, low_bat;
  25. int left_motor, throttle_left_motor, throttle_counter_left_motor, throttle_left_motor_memory;
  26. int right_motor, throttle_right_motor, throttle_counter_right_motor, throttle_right_motor_memory;
  27. int battery_voltage;
  28. int receive_counter;
  29. int gyro_pitch_data_raw, gyro_yaw_data_raw, accelerometer_data_raw;
  30. long gyro_yaw_calibration_value, gyro_pitch_calibration_value;
  31. unsigned long loop_timer;
  32. float angle_gyro, angle_acc, angle, self_balance_pid_setpoint;
  33. float pid_error_temp, pid_i_mem, pid_setpoint, gyro_input, pid_output, pid_last_d_error;
  34. float pid_output_left, pid_output_right;
  35. ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  36. //Setup basic functions
  37. ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  38. void setup(){
  39.   Serial.begin(9600);                                                       //Start the serial port at 9600 kbps
  40.   Wire.begin();                                                             //Start the I2C bus as master
  41.   TWBR = 12;                                                                //Set the I2C clock speed to 400kHz
  42.   //To create a variable pulse for controlling the stepper motors a timer is created that will execute a piece of code (subroutine) every 20us
  43.   //This subroutine is called TIMER2_COMPA_vect
  44.   TCCR2A = 0;                                                               //Make sure that the TCCR2A register is set to zero
  45.   TCCR2B = 0;                                                               //Make sure that the TCCR2A register is set to zero
  46.   TIMSK2 |= (1 << OCIE2A);                                                  //Set the interupt enable bit OCIE2A in the TIMSK2 register
  47.   TCCR2B |= (1 << CS21);                                                    //Set the CS21 bit in the TCCRB register to set the prescaler to 8
  48.   OCR2A = 39;                                                               //The compare register is set to 39 => 20us / (1s / (16.000.000MHz / 8)) - 1
  49.   TCCR2A |= (1 << WGM21);                                                   //Set counter 2 to CTC (clear timer on compare) mode
  50.   
  51.   //By default the MPU-6050 sleeps. So we have to wake it up.
  52.   Wire.beginTransmission(gyro_address);                                     //Start communication with the address found during search.
  53.   Wire.write(0x6B);                                                         //We want to write to the PWR_MGMT_1 register (6B hex)
  54.   Wire.write(0x00);                                                         //Set the register bits as 00000000 to activate the gyro
  55.   Wire.endTransmission();                                                   //End the transmission with the gyro.
  56.   //Set the full scale of the gyro to +/- 250 degrees per second
  57.   Wire.beginTransmission(gyro_address);                                     //Start communication with the address found during search.
  58.   Wire.write(0x1B);                                                         //We want to write to the GYRO_CONFIG register (1B hex)
  59.   Wire.write(0x00);                                                         //Set the register bits as 00000000 (250dps full scale)
  60.   Wire.endTransmission();                                                   //End the transmission with the gyro
  61.   //Set the full scale of the accelerometer to +/- 4g.
  62.   Wire.beginTransmission(gyro_address);                                     //Start communication with the address found during search.
  63.   Wire.write(0x1C);                                                         //We want to write to the ACCEL_CONFIG register (1A hex)
  64.   Wire.write(0x08);                                                         //Set the register bits as 00001000 (+/- 4g full scale range)
  65.   Wire.endTransmission();                                                   //End the transmission with the gyro
  66.   //Set some filtering to improve the raw data.
  67.   Wire.beginTransmission(gyro_address);                                     //Start communication with the address found during search
  68.   Wire.write(0x1A);                                                         //We want to write to the CONFIG register (1A hex)
  69.   Wire.write(0x03);                                                         //Set the register bits as 00000011 (Set Digital Low Pass Filter to ~43Hz)
  70.   Wire.endTransmission();                                                   //End the transmission with the gyro
  71.   pinMode(2, OUTPUT);                                                       //Configure digital poort 2 as output
  72.   pinMode(3, OUTPUT);                                                       //Configure digital poort 3 as output
  73.   pinMode(4, OUTPUT);                                                       //Configure digital poort 4 as output
  74.   pinMode(5, OUTPUT);                                                       //Configure digital poort 5 as output
  75.   pinMode(13, OUTPUT);                                                      //Configure digital poort 6 as output
  76.   for(receive_counter = 0; receive_counter < 500; receive_counter++){       //Create 500 loops
  77.     if(receive_counter % 15 == 0)digitalWrite(13, !digitalRead(13));        //Change the state of the LED every 15 loops to make the LED blink fast
  78.     Wire.beginTransmission(gyro_address);                                   //Start communication with the gyro
  79.     Wire.write(0x43);                                                       //Start reading the Who_am_I register 75h
  80.     Wire.endTransmission();                                                 //End the transmission
  81.     Wire.requestFrom(gyro_address, 4);                                      //Request 2 bytes from the gyro
  82.     gyro_yaw_calibration_value += Wire.read()<<8|Wire.read();               //Combine the two bytes to make one integer
  83.     gyro_pitch_calibration_value += Wire.read()<<8|Wire.read();             //Combine the two bytes to make one integer
  84.     delayMicroseconds(3700);                                                //Wait for 3700 microseconds to simulate the main program loop time
  85.   }
  86.   gyro_pitch_calibration_value /= 500;                                      //Divide the total value by 500 to get the avarage gyro offset
  87.   gyro_yaw_calibration_value /= 500;                                        //Divide the total value by 500 to get the avarage gyro offset
  88.   loop_timer = micros() + 4000;                                             //Set the loop_timer variable at the next end loop time
  89. }
  90. ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  91. //Main program loop
  92. ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  93. void loop(){
  94.   if(Serial.available()){                                                   //If there is serial data available
  95.     received_byte = Serial.read();                                          //Load the received serial data in the received_byte variable
  96.     receive_counter = 0;                                                    //Reset the receive_counter variable
  97.   }
  98.   if(receive_counter <= 25)receive_counter ++;                              //The received byte will be valid for 25 program loops (100 milliseconds)
  99.   else received_byte = 0x00;                                                //After 100 milliseconds the received byte is deleted
  100.   
  101.   //Load the battery voltage to the battery_voltage variable.
  102.   //85 is the voltage compensation for the diode.
  103.   //Resistor voltage divider => (3.3k + 3.3k)/2.2k = 2.5
  104.   //12.5V equals ~5V [url=home.php?mod=space&uid=870029]@[/url] Analog 0.
  105.   //12.5V equals 1023 analogRead(0).
  106.   //1250 / 1023 = 1.222.
  107.   //The variable battery_voltage holds 1050 if the battery voltage is 10.5V.
  108.   battery_voltage = (analogRead(0) * 1.222) + 85;
  109.   
  110.   if(battery_voltage < 1050 && battery_voltage > 800){                      //If batteryvoltage is below 10.5V and higher than 8.0V
  111.     digitalWrite(13, HIGH);                                                 //Turn on the led if battery voltage is to low
  112.     low_bat = 1;                                                            //Set the low_bat variable to 1
  113.   }
  114.   ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  115.   //Angle calculations
  116.   ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  117.   Wire.beginTransmission(gyro_address);                                     //Start communication with the gyro
  118.   Wire.write(0x3F);                                                         //Start reading at register 3F
  119.   Wire.endTransmission();                                                   //End the transmission
  120.   Wire.requestFrom(gyro_address, 2);                                        //Request 2 bytes from the gyro
  121.   accelerometer_data_raw = Wire.read()<<8|Wire.read();                      //Combine the two bytes to make one integer
  122.   accelerometer_data_raw += acc_calibration_value;                          //Add the accelerometer calibration value
  123.   if(accelerometer_data_raw > 8200)accelerometer_data_raw = 8200;           //Prevent division by zero by limiting the acc data to +/-8200;
  124.   if(accelerometer_data_raw < -8200)accelerometer_data_raw = -8200;         //Prevent division by zero by limiting the acc data to +/-8200;
  125.   angle_acc = asin((float)accelerometer_data_raw/8200.0)* 57.296;           //Calculate the current angle according to the accelerometer
  126.   if(start == 0 && angle_acc > -0.5&& angle_acc < 0.5){                     //If the accelerometer angle is almost 0
  127.     angle_gyro = angle_acc;                                                 //Load the accelerometer angle in the angle_gyro variable
  128.     start = 1;                                                              //Set the start variable to start the PID controller
  129.   }
  130.   
  131.   Wire.beginTransmission(gyro_address);                                     //Start communication with the gyro
  132.   Wire.write(0x43);                                                         //Start reading at register 43
  133.   Wire.endTransmission();                                                   //End the transmission
  134.   Wire.requestFrom(gyro_address, 4);                                        //Request 4 bytes from the gyro
  135.   gyro_yaw_data_raw = Wire.read()<<8|Wire.read();                           //Combine the two bytes to make one integer
  136.   gyro_pitch_data_raw = Wire.read()<<8|Wire.read();                         //Combine the two bytes to make one integer
  137.   
  138.   gyro_pitch_data_raw -= gyro_pitch_calibration_value;                      //Add the gyro calibration value
  139.   angle_gyro += gyro_pitch_data_raw * 0.000031;                             //Calculate the traveled during this loop angle and add this to the angle_gyro variable
  140.   
  141.   ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  142.   //MPU-6050 offset compensation
  143.   ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  144.   //Not every gyro is mounted 100% level with the axis of the robot. This can be cause by misalignments during manufacturing of the breakout board.
  145.   //As a result the robot will not rotate at the exact same spot and start to make larger and larger circles.
  146.   //To compensate for this behavior a VERY SMALL angle compensation is needed when the robot is rotating.
  147.   //Try 0.0000003 or -0.0000003 first to see if there is any improvement.
  148.   gyro_yaw_data_raw -= gyro_yaw_calibration_value;                          //Add the gyro calibration value
  149.   //Uncomment the following line to make the compensation active
  150.   //angle_gyro -= gyro_yaw_data_raw * 0.0000003;                            //Compensate the gyro offset when the robot is rotating
  151.   angle_gyro = angle_gyro * 0.9996 + angle_acc * 0.0004;                    //Correct the drift of the gyro angle with the accelerometer angle
  152.   ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  153.   //PID controller calculations
  154.   ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  155.   //The balancing robot is angle driven. First the difference between the desired angel (setpoint) and actual angle (process value)
  156.   //is calculated. The self_balance_pid_setpoint variable is automatically changed to make sure that the robot stays balanced all the time.
  157.   //The (pid_setpoint - pid_output * 0.015) part functions as a brake function.
  158.   pid_error_temp = angle_gyro - self_balance_pid_setpoint - pid_setpoint;
  159.   if(pid_output > 10 || pid_output < -10)pid_error_temp += pid_output * 0.015 ;
  160.   pid_i_mem += pid_i_gain * pid_error_temp;                                 //Calculate the I-controller value and add it to the pid_i_mem variable
  161.   if(pid_i_mem > 400)pid_i_mem = 400;                                       //Limit the I-controller to the maximum controller output
  162.   else if(pid_i_mem < -400)pid_i_mem = -400;
  163.   //Calculate the PID output value
  164.   pid_output = pid_p_gain * pid_error_temp + pid_i_mem + pid_d_gain * (pid_error_temp - pid_last_d_error);
  165.   if(pid_output > 400)pid_output = 400;                                     //Limit the PI-controller to the maximum controller output
  166.   else if(pid_output < -400)pid_output = -400;
  167.   pid_last_d_error = pid_error_temp;                                        //Store the error for the next loop
  168.   if(pid_output < 5 && pid_output > -5)pid_output = 0;                      //Create a dead-band to stop the motors when the robot is balanced
  169.   if(angle_gyro > 30 || angle_gyro < -30 || start == 0 || low_bat == 1){    //If the robot tips over or the start variable is zero or the battery is empty
  170.     pid_output = 0;                                                         //Set the PID controller output to 0 so the motors stop moving
  171.     pid_i_mem = 0;                                                          //Reset the I-controller memory
  172.     start = 0;                                                              //Set the start variable to 0
  173.     self_balance_pid_setpoint = 0;                                          //Reset the self_balance_pid_setpoint variable
  174.   }
  175.   ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  176.   //Control calculations
  177.   ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  178.   pid_output_left = pid_output;                                             //Copy the controller output to the pid_output_left variable for the left motor
  179.   pid_output_right = pid_output;                                            //Copy the controller output to the pid_output_right variable for the right motor
  180.   if(received_byte & B00000001){                                            //If the first bit of the receive byte is set change the left and right variable to turn the robot to the left
  181.     pid_output_left += turning_speed;                                       //Increase the left motor speed
  182.     pid_output_right -= turning_speed;                                      //Decrease the right motor speed
  183.   }
  184.   if(received_byte & B00000010){                                            //If the second bit of the receive byte is set change the left and right variable to turn the robot to the right
  185.     pid_output_left -= turning_speed;                                       //Decrease the left motor speed
  186.     pid_output_right += turning_speed;                                      //Increase the right motor speed
  187.   }
  188.   if(received_byte & B00000100){                                            //If the third bit of the receive byte is set change the left and right variable to turn the robot to the right
  189.     if(pid_setpoint > -2.5)pid_setpoint -= 0.05;                            //Slowly change the setpoint angle so the robot starts leaning forewards
  190.     if(pid_output > max_target_speed * -1)pid_setpoint -= 0.005;            //Slowly change the setpoint angle so the robot starts leaning forewards
  191.   }
  192.   if(received_byte & B00001000){                                            //If the forth bit of the receive byte is set change the left and right variable to turn the robot to the right
  193.     if(pid_setpoint < 2.5)pid_setpoint += 0.05;                             //Slowly change the setpoint angle so the robot starts leaning backwards
  194.     if(pid_output < max_target_speed)pid_setpoint += 0.005;                 //Slowly change the setpoint angle so the robot starts leaning backwards
  195.   }   
  196.   if(!(received_byte & B00001100)){                                         //Slowly reduce the setpoint to zero if no foreward or backward command is given
  197.     if(pid_setpoint > 0.5)pid_setpoint -=0.05;                              //If the PID setpoint is larger then 0.5 reduce the setpoint with 0.05 every loop
  198.     else if(pid_setpoint < -0.5)pid_setpoint +=0.05;                        //If the PID setpoint is smaller then -0.5 increase the setpoint with 0.05 every loop
  199.     else pid_setpoint = 0;                                                  //If the PID setpoint is smaller then 0.5 or larger then -0.5 set the setpoint to 0
  200.   }
  201.   
  202.   //The self balancing point is adjusted when there is not forward or backwards movement from the transmitter. This way the robot will always find it's balancing point
  203.   if(pid_setpoint == 0){                                                    //If the setpoint is zero degrees
  204.     if(pid_output < 0)self_balance_pid_setpoint += 0.0015;                  //Increase the self_balance_pid_setpoint if the robot is still moving forewards
  205.     if(pid_output > 0)self_balance_pid_setpoint -= 0.0015;                  //Decrease the self_balance_pid_setpoint if the robot is still moving backwards
  206.   }
  207.   ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  208.   //Motor pulse calculations
  209.   ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  210.   //To compensate for the non-linear behaviour of the stepper motors the folowing calculations are needed to get a linear speed behaviour.
  211.   if(pid_output_left > 0)pid_output_left = 405 - (1/(pid_output_left + 9)) * 5500;
  212.   else if(pid_output_left < 0)pid_output_left = -405 - (1/(pid_output_left - 9)) * 5500;
  213.   if(pid_output_right > 0)pid_output_right = 405 - (1/(pid_output_right + 9)) * 5500;
  214.   else if(pid_output_right < 0)pid_output_right = -405 - (1/(pid_output_right - 9)) * 5500;
  215.   //Calculate the needed pulse time for the left and right stepper motor controllers
  216.   if(pid_output_left > 0)left_motor = 400 - pid_output_left;
  217.   else if(pid_output_left < 0)left_motor = -400 - pid_output_left;
  218.   else left_motor = 0;
  219.   if(pid_output_right > 0)right_motor = 400 - pid_output_right;
  220.   else if(pid_output_right < 0)right_motor = -400 - pid_output_right;
  221.   else right_motor = 0;
  222.   //Copy the pulse time to the throttle variables so the interrupt subroutine can use them
  223.   throttle_left_motor = left_motor;
  224.   throttle_right_motor = right_motor;
  225.   ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  226.   //Loop time timer
  227.   ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  228.   //The angle calculations are tuned for a loop time of 4 milliseconds. To make sure every loop is exactly 4 milliseconds a wait loop
  229.   //is created by setting the loop_timer variable to +4000 microseconds every loop.
  230.   while(loop_timer > micros());
  231.   loop_timer += 4000;
  232. }
  233. ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  234. //Interrupt routine  TIMER2_COMPA_vect
  235. ///////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
  236. ISR(TIMER2_COMPA_vect){
  237.   //Left motor pulse calculations
  238.   throttle_counter_left_motor ++;                                           //Increase the throttle_counter_left_motor variable by 1 every time this routine is executed
  239.   if(throttle_counter_left_motor > throttle_left_motor_memory){             //If the number of loops is larger then the throttle_left_motor_memory variable
  240.     throttle_counter_left_motor = 0;                                        //Reset the throttle_counter_left_motor variable
  241.     throttle_left_motor_memory = throttle_left_motor;                       //Load the next throttle_left_motor variable
  242.     if(throttle_left_motor_memory < 0){                                     //If the throttle_left_motor_memory is negative
  243.       PORTD &= 0b11110111;                                                  //Set output 3 low to reverse the direction of the stepper controller
  244.       throttle_left_motor_memory *= -1;                                     //Invert the throttle_left_motor_memory variable
  245.     }
  246.     else PORTD |= 0b00001000;                                               //Set output 3 high for a forward direction of the stepper motor
  247.   }
  248.   else if(throttle_counter_left_motor == 1)PORTD |= 0b00000100;             //Set output 2 high to create a pulse for the stepper controller
  249.   else if(throttle_counter_left_motor == 2)PORTD &= 0b11111011;             //Set output 2 low because the pulse only has to last for 20us
  250.   
  251.   //right motor pulse calculations
  252.   throttle_counter_right_motor ++;                                          //Increase the throttle_counter_right_motor variable by 1 every time the routine is executed
  253.   if(throttle_counter_right_motor > throttle_right_motor_memory){           //If the number of loops is larger then the throttle_right_motor_memory variable
  254.     throttle_counter_right_motor = 0;                                       //Reset the throttle_counter_right_motor variable
  255.     throttle_right_motor_memory = throttle_right_motor;                     //Load the next throttle_right_motor variable
  256.     if(throttle_right_motor_memory < 0){                                    //If the throttle_right_motor_memory is negative
  257.       PORTD |= 0b00100000;                                                  //Set output 5 low to reverse the direction of the stepper controller
  258.       throttle_right_motor_memory *= -1;                                    //Invert the throttle_right_motor_memory variable
  259.     }
  260.     else PORTD &= 0b11011111;                                               //Set output 5 high for a forward direction of the stepper motor
  261.   }
  262.   else if(throttle_counter_right_motor == 1)PORTD |= 0b00010000;            //Set output 4 high to create a pulse for the stepper controller
  263.   else if(throttle_counter_right_motor == 2)PORTD &= 0b11101111;            //Set output 4 low because the pulse only has to last for 20us
  264. }
复制代码


回复

使用道具 举报

驴友花雕  中级技神
 楼主|

发表于 4 小时前

【Arduino 动手做】YABR:一款使用步进电机的自平衡机器人

【Arduino 动手做】YABR:一款使用步进电机的自平衡机器人
项目链接:http://www.brokking.net/yabr_main.html
项目作者:Joop Brokking

项目视频 :https://www.youtube.com/watch?v=6WWqo-Yr8lA
项目代码:http://77.161.83.3/cgi-bin/yabr.cgi

【Arduino 动手做】YABR:一款使用步进电机的自平衡机器人图2

【Arduino 动手做】YABR:一款使用步进电机的自平衡机器人图1

回复

使用道具 举报

您需要登录后才可以回帖 登录 | 立即注册

本版积分规则

为本项目制作心愿单
购买心愿单
心愿单 编辑
[[wsData.name]]

硬件清单

  • [[d.name]]
btnicon
我也要做!
点击进入购买页面
上海智位机器人股份有限公司 沪ICP备09038501号-4 备案 沪公网安备31011502402448

© 2013-2025 Comsenz Inc. Powered by Discuz! X3.4 Licensed

mail