嘿,伙计们。我知道已经有很多平衡机器人项目在互联网上，但我也想做一个。事实上，我想做一些特别的东西，使一个迷你平衡机器人，但我有点失败与继电机，我会解释下面的视频的问题。因此，我最后所做的，是使用相同的PCB，我已经准备了一个迷你平衡机器人，设计一个小的3D打印体，并使用更大的步进电机，这样能够完成我的想法。机器人应该从带有蓝牙连接的自制遥控器获取信息，这样我们就可以四处走动了。但在内部，机器人也应该从IMU模块获取信息，并计算角度的PID值，这样它就不会下降，总是试图保持水平。这是非常有趣的，我想你会学到很多与这个项目，因为我将解释电路的每一步，但也代码。那么，你觉得，这行得通吗？因此，让我们开始吧。
第 1 部分 - 为什么不是迷你机器人呢？
我的计划是使用这个项目的PCB与这种步进电机，你可以看到下面。我有这些从旧的DVD驱动器， 我认为它们还可以在我的PCB上很好的工作 。很容易用步进电机驱动程序控制它们， 所以这不是问题所在。问题是机械的。你看，这些电机有一个轴连接到里面的磁铁。此轴由两侧支撑。为了给这个轴增加一个轮子，我不得不把它剪小，这样，我们只有一边的支持，这就是问题所在。没有两侧的支持，这个轴将移动，并自动打破里面所有的小线圈和磁铁。我试过在输出时粘合轴承，但没有成功。里面，轴被一个金属球保持在中心，一旦你移动，你可以告别孔电机。所以我别无选择，只能使用NEMA 17电机。因此，在PCB的一侧，我设计了一个3D打印支持。
第 2 部分 - 我们需要什么
让我们看看这个项目需要什么。首先，PCB。如果您想要相同的 PCB，请从下面的一章中获取 GERBER 文件。我们需要的步进电机驱动程序，这是基于A4988的，但更妙的是，TMC2225驱动器可以更好和无声的运动。我们需要两个 NEMA 17 电机，MPU6050 陀螺仪/acc 模块，一个小型 5V 降压转换器，一个 3S 电池，两个 HC-05 蓝牙模块，一个用于机器人，另一个用于遥控器。此外，我们还可以添加一个小的OLED屏幕，以便为机器人创建人脸。作为小弹头，我们需要一个ATMega328p-AU芯片，16MHz谐振器和一些SMD电阻器和电容器。还有一个小的SMD蜂鸣器和一些LED。
第三部分 原理图
第四部分 PCB
GERBERs.zip
(112.87 KB)
第五部分 组装其他
3D打印文件
3D_STL.zip
(571.76 KB)
第6部分 - 机器人代码
代码几乎和我们对于无刷PID控制器或乒乓球控制器的代码相同，但这次我们使用步进电机。顶部创建应用于继器驱动程序的步进脉冲，在这种情况下，我们使用时间器 TIEMR 2。通过设置一些寄存器，我们使计时器每 20us 单击一次。我们阅读陀螺仪和加速度计的原始数据。为了克服陀螺偏移，在乞讨时，我们从 IMU 模块中读取大约 500 条读取，并将该数据保存为偏移，并在主循环中减去该数据。然后，我们计算每个循环的角度，该角度设置为每个 4000us。使用"gyro_angle"变量，我们可以计算 PID 值。和往常一样，我们发现PID错误是机器人的真实角度和所需角度之间的差异，后者应该是0，这样机器人就可以保持水平。最后，在计时器例程中，根据 PID 输出，我们通过创建或多或少、更快或更慢的应用于继电机的脉冲来计算电机的速度。为了加快代码的速度，我们使用注册控制，例如：PORTD |= 0b00001000;与将数字引脚 D3 设置为 HIGH 相同。有关此的更多信息，请查看我的Arduino 注册控制视频。
我的机器人还在晃来晃
去。这是因为我们必须调整代码中的 PID 值。这是这个项目中最长的部分。我不得不手动去改变PID值，直到我得到更好的结果，即使如此，我不太满意的最终结果。如果你只从P值开始，I和D是0，你会得到某种振荡，车轮根据角度与机器人的倾斜方向旋转。然后，我们添加一些 D 值，它会对角度变化的速度做出反应，并尝试停止振荡，但当误差非常小时，这还不够。最后，为了精细调整位置，我们添加的 I 在错误小时会缓慢增加或减少。超过30次尝试，我有或多或少的好结果，机器人是相当稳定的，但我还没有完全高兴。
/* This code is for the balancing robot project. You can find more about this on https://www.electronoobs.com.
* The code will read data from an IMU module, calculate the PID value in order to balance the robot at 0ยบ and
* then it will create pules using a timer and apply those to some stepper motors and move the robot.
* Kind thanks to Joop Brokking for the help: https://www.youtube.com/user/MacPuffdog
*
* Tutorial: https://electronoobs.com/eng_arduino_tut159.php
* Schematic: https://electronoobs.com/eng_arduino_tut150_sch1.php */
//Include Libraries
#include <Wire.h> //Wire.h is used to get i2c data from the MPU6050
/////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////PID VALUES////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////
float Kp = 30; //P Gain; Mine was 30
float Ki = 0.61; //I Gain; Mine was 0.61
float Kd = 9; //D Gain; Mine was 9
float Moving_Speed = 20; //Moving speed with Bluetooth Control; Mine was 20
float Max_Speed = 160; //Max mooving speed; Mine was 160
int Acc_Offset = 1045; //Accelerometer offset value (find this before you run the code)
/////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////VARIABLES////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////
byte Activated, Received_byte;
int left_motor;
int Left_Motor_Speed;
int CC_Speed_Left_Motor;
int Left_Motor_Speed_Prev;
int right_motor;
int Right_Motor_Speed;
int CC_Speed_Right_Motor;
int Right_Motor_Speed_Prev;
int Received_Since;
int Gyro_X_Raw, Gyro_Y_Raw, Acc_Raw;
long Gyro_Y_Offset, Gyro_X_Offset;
unsigned long Loop_Time;
float Gyro_Angle, Acc_Angle, Auto_Setpoint;
float Temp_Error, PID_I, Setpoint, gyro_input, PID_Value, Last_D_Error;
float PID_Value_left, PID_Value_right;
int MPU6050_ADDR = 0x68; //MPU6050 I2C address (0x68 or sometimes 0x69)
/////////////////////////////////////////////////////////////////////////////////////////////////
//////////////////////////////////////////Input/Output///////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////
int DIR_L = 2; //Pin for left driver direction pin
int STEP_L = 3; //Pin for left driver steps pin
int DIR_R = 4; //Pin for right driver direction pin
int STEP_R = 5; //Pin for right driver steps pin
int Enable = 6; //Pin for drivers enable (both use same pin)
int LED1 = 7; //Left LED is connected on D7
int LED2 = 8; //Right LED is connected on D8
int Buzzer = 9; //Buzzer is connected on D9
/////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////SETUP LOOP////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////
void setup() {
Serial.begin(9600); //Start the serial port at 9600 kbps
Wire.begin(); //Start I2C communication
TWBR = 12; //Also set I2C clock speed to 400kHz
//Define the pins mode (Output or Input)
pinMode(DIR_L, OUTPUT);
pinMode(STEP_L, OUTPUT);
pinMode(DIR_R, OUTPUT);
pinMode(STEP_R, OUTPUT);
pinMode(LED1, OUTPUT);
pinMode(LED2, OUTPUT);
pinMode(Buzzer, OUTPUT);
/*Read this: Now we define a "timer", timer 2 in this case. This timer will be used to create
the step pulses for the motors. This timer will click every 20us and make the required calculations.
The code will be inside the subroutine of TIMER2_COMPA_vect at the end of the code*/
TCCR2A = 0; //Start with TCCR2A set to zero
TCCR2B = 0; //Start with TCCR2B set to zero
TIMSK2 |= (1 << OCIE2A); //Interupt enable bit OCIE2A set to 1
TCCR2B |= (1 << CS21); //Set CS21 bit: We set prescaler to 8
OCR2A = 39; //Compare register is 39, so... 20us/(1s/(16MHz/8))-1
TCCR2A |= (1 << WGM21); //Mode: Clear timer on compare
//Start MPU6050 communication
Wire.beginTransmission(MPU6050_ADDR); //From the datastheet, the address is 0x68, but you can change that above.
Wire.write(0x6B); //Write on 0x6B register
Wire.write(0x00); //Set register to 00000000 and activate gyro
Wire.endTransmission(); //End the i2c transmission
//Change gyro scale to +/-250deg/sec
Wire.beginTransmission(MPU6050_ADDR); //My MPU6050 address is 0x68, change it at the begginning of the code
Wire.write(0x1B); //Write on 0x1B register
Wire.write(0x00); //Set scale to 250dps, full scale
Wire.endTransmission(); //End the i2c transmission
//Change accelerometer scale to +/-4g.
Wire.beginTransmission(MPU6050_ADDR); //My MPU6050 address is 0x68, change it at the begginning of the code
Wire.write(0x1C); //Write on 0x1C register
Wire.write(0x08); //Set scale to +/-4g
Wire.endTransmission(); //End the i2c transmission
//Enable some filters
Wire.beginTransmission(MPU6050_ADDR); //My MPU6050 address is 0x68, change it at the begginning of the code
Wire.write(0x1A); //Write on 0x1A register
Wire.write(0x03); //Set Digital Low Pass Filter to ~43Hz
Wire.endTransmission(); //End the i2c transmission
/*When we start, the gyro might have an offset value. We make 520 readdings and get that calibration value
We use taht later in the code to substract the raw offset. */
for (int i = 0; i < 520; i++) { //Create 520 loops
if (i % 20 == 0){
digitalWrite(LED1, !digitalRead(LED1)); //Blink the LED every 20 loops
digitalWrite(Buzzer, !digitalRead(Buzzer)); //Buzz every 20 loops
}
Wire.beginTransmission(MPU6050_ADDR); //Start i2c communication with MPU6050
Wire.write(0x43); //We read from register 0x43
Wire.endTransmission(); //End the i2c transmission
Wire.requestFrom(MPU6050_ADDR, 4); //Request 2 bytes from the MPU6050
Gyro_Y_Offset += Wire.read() << 8 | Wire.read(); //Merge high and low byte and get an integer
Gyro_X_Offset += Wire.read() << 8 | Wire.read(); //Merge high and low byte and get an integer
delayMicroseconds(3500); //Small delay
}
Gyro_X_Offset /= 520; //Divide the total value by 520 to get the avarage gyro offset
Gyro_Y_Offset /= 520; //Divide the total value by 520 to get the avarage gyro offset
delay(200); //Small Delay
pinMode(Enable, OUTPUT); //Set Enable pin as OUTPUT
digitalWrite(Enable, LOW); //Finally, we enable the stepper drivers (drivers are enabled with LOW)
//Set the Loop_Time variable at the next end loop time
Loop_Time = micros() + 4000; //Loop time is 4000us
}
/////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////VOID LOOP////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////
void loop() {
if (Serial.available()) { //If there is serial data available from HC-05 module
Received_byte = Serial.read(); //Store the received data
Received_Since = 0; //Reset the counter
}
if (Received_Since <= 25) {
Received_Since ++; //The received data wwill last 25 loops, around 100 milliseconds
}
else Received_byte = 0x00; //After 100ms we reset the received data
/////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////CALCUALTE ANGLE/////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////
Wire.beginTransmission(MPU6050_ADDR); //Start communication with MPU6050
Wire.write(0x3F); //Start reading register 3F
Wire.endTransmission(); //End i2c transmission
Wire.requestFrom(MPU6050_ADDR, 2); //Request 2 bytes from the MPU6050
Acc_Raw = Wire.read() << 8 | Wire.read(); //Merge high and low byte and get an integer
Acc_Raw += Acc_Offset; //Add the accelerometer offset value
if (Acc_Raw > 8200)Acc_Raw = 8200; //Prevent division by zero by limiting the acc data to +/-8200;
if (Acc_Raw < -8200)Acc_Raw = -8200; //Prevent division by zero by limiting the acc data to +/-8200;
Acc_Angle = asin((float)Acc_Raw / 8200.0) * 57.296; //Calculate the current angle according to the accelerometer data
if (Activated == 0 && Acc_Angle > -0.5 && Acc_Angle < 0.5) { //If the accelerometer angle is almost 0
Gyro_Angle = Acc_Angle; //Load the accelerometer angle in the Gyro_Angle variable
Activated = 1; //Set "Activated" variable and start PID control
}
Wire.beginTransmission(MPU6050_ADDR); //Start communication with MPU6050
Wire.write(0x43); //Start reading register 43
Wire.endTransmission(); //End i2c transmission
Wire.requestFrom(MPU6050_ADDR, 4); //Request 4 bytes from the gyro
Gyro_Y_Raw = Wire.read() << 8 | Wire.read(); ////Merge high and low byte and get an integer
Gyro_X_Raw = Wire.read() << 8 | Wire.read(); ////Merge high and low byte and get an integer
Gyro_X_Raw -= Gyro_X_Offset; //Add the gyro offset value
Gyro_Angle += Gyro_X_Raw * 0.000031; //Calculate traveled angle during this loop
Gyro_Y_Raw -= Gyro_Y_Offset; //Add gyro offset value
Gyro_Angle = Gyro_Angle * 0.9996 + Acc_Angle * 0.0004; //Correct the drift of the gyro angle with the accelerometer angle
/////////////////////////////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////PID CONTROL///////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////
/*PID control si almos alwasy the same. We have a setpoint, which is the desired angle in this case (horizontal).
The PID will adjsut the speed and direction of the motors, so we can always go towards the desired angle. To get
the PID value, we use a PID algorithm formula.*/
//First, we calculate the error between the real angle and the value taht we want, in this case would be 0ยบ
Temp_Error = Gyro_Angle - Auto_Setpoint - Setpoint;
if (PID_Value > 10 || PID_Value < -10) {
Temp_Error += PID_Value * 0.015 ;
}
//I value
PID_I += Ki * Temp_Error; //Calculate the "I" value
if (PID_I > 400)PID_I = 400; //We limit the "I" to the maximum output
else if (PID_I < -400)PID_I = -400;
//Calculate the PID output value
PID_Value = Kp * Temp_Error + PID_I + Kd * (Temp_Error - Last_D_Error);
if (PID_Value > 400)PID_Value = 400; //Limit the P+I to the maximum output
else if (PID_Value < -400)PID_Value = -400;
Last_D_Error = Temp_Error; //Store the error for the next loop
if (PID_Value < 6 && PID_Value > - 6)PID_Value = 0; //Dead-band where the robot is more or less balanced
if (Gyro_Angle > 30 || Gyro_Angle < -30 || Activated == 0) { //If the robot falls or the "Activated" is 0
PID_Value = 0; //Set the PID output to 0 so the motors are stopped
PID_I = 0; //Reset the I-controller memory
Activated = 0; //Set the Activated variable to 0
Auto_Setpoint = 0; //Reset the Auto_Setpoint variable
}
/////////////////////////////////////////////////////////////////////////////////////////////////
/////////////////////////////////////////HC-05 CONTROL///////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////
PID_Value_left = PID_Value; //Get PID output for the left motor
PID_Value_right = PID_Value; //Get PID output for the right motor
if (Received_byte & B00000001) { //We receive a a 00000001 so we turn Right
PID_Value_left += Moving_Speed; //Increase the left motor speed
PID_Value_right -= Moving_Speed; //Decrease the right motor speed
}
if (Received_byte & B00000010) { //We receive a a 00000010 so we turn Left
PID_Value_left -= Moving_Speed; //Decrease the left motor speed
PID_Value_right += Moving_Speed; //Increase the right motor speed
}
if (Received_byte & B00000100) { //We receive a a 00000100 so we go forward
if (Setpoint > -2.5)Setpoint -= 0.05; //Change the setpoint angle so the robot leans forwards
if (PID_Value > Max_Speed * -1)Setpoint -= 0.005; //Change the setpoint angle so the robot leans forwards
}
if (Received_byte & B00001000) { //We receive a a 00001000 so we go backwards
if (Setpoint < 2.5)Setpoint += 0.05; //Change the setpoint angle so the robot leans backwards
if (PID_Value < Max_Speed)Setpoint += 0.005; //Change the setpoint angle so the robot leans backwards
}
if (!(Received_byte & B00001100)) { //We receive a a 00001100 so no movement
if (Setpoint > 0.5)Setpoint -= 0.05; //If the PID setpoint is higher than 0.5, reduce setpoint by 0.05 every loop
else if (Setpoint < -0.5)Setpoint += 0.05; //If the PID setpoint is lower than -0.5, increase setpoint by 0.05 every loop
else Setpoint = 0; //If the PID setpoint is lower than 0.5 or highert than -0.5, set the setpoint to 0
}
if (Setpoint == 0) { //If the setpoint is zero degrees
if (PID_Value < 0)Auto_Setpoint += 0.001; //Increase the Auto_Setpoint if the robot is still moving forewards
if (PID_Value > 0)Auto_Setpoint -= 0.001; //Decrease the Auto_Setpoint if the robot is still moving backwards
}
/////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////MOTORS CONTROL///////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////
if (PID_Value_left > 0){
PID_Value_left = 405 - (1 / (PID_Value_left + 9)) * 5500;
}
else if (PID_Value_left < 0){
PID_Value_left = -405 - (1 / (PID_Value_left - 9)) * 5500;
}
if (PID_Value_right > 0){
PID_Value_right = 405 - (1 / (PID_Value_right + 9)) * 5500;
}
else if (PID_Value_right < 0){
PID_Value_right = -405 - (1 / (PID_Value_right - 9)) * 5500;
}
//Calculate the pulse time for the left and right motor
if (PID_Value_left > 0){
left_motor = 400 - PID_Value_left;
}
else if (PID_Value_left < 0){
left_motor = -400 - PID_Value_left;
}
else left_motor = 0;
if (PID_Value_right > 0){
right_motor = 400 - PID_Value_right;
}
else if (PID_Value_right < 0){
right_motor = -400 - PID_Value_right;
}
else right_motor = 0;
Left_Motor_Speed = left_motor;
Right_Motor_Speed = right_motor;
/*The angle calculations are tuned for a loop time of 4 milliseconds.
//We make sure every loop is exactly 4 milliseconds so we set the Loop_Time to +4000 microseconds every loop.*/
while (Loop_Time > micros());
Loop_Time += 4000;
}//End of void loop
/////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////TIMER2_COMPA_vect////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////
ISR(TIMER2_COMPA_vect) {
//Left motor pulses
CC_Speed_Left_Motor ++; //Increase CC_Speed_Left_Motor by 1 every time this routine is executed
if (CC_Speed_Left_Motor > Left_Motor_Speed_Prev) { //If the number of loops is larger then the Left_Motor_Speed_Prev variable
CC_Speed_Left_Motor = 0; //Reset the CC_Speed_Left_Motor variable
Left_Motor_Speed_Prev = Left_Motor_Speed; //Load the next Left_Motor_Speed variable
if (Left_Motor_Speed_Prev < 0) { //If the Left_Motor_Speed_Prev is negative
PORTD &= 0b11111011; //Set D2 low. Reverse direction
Left_Motor_Speed_Prev *= -1; //Invert the Left_Motor_Speed_Prev variable
}
else PORTD |= 0b00000100; //Set output D2 high. Forward direction.
}
else if (CC_Speed_Left_Motor == 1)PORTD |= 0b00001000; //Set output D3 high to create a pulse for the stepper
else if (CC_Speed_Left_Motor == 2)PORTD &= 0b11110111; //Set output D3 low because the pulse only has to last for 20us
//Right motor pulses
CC_Speed_Right_Motor ++; //Increase CC_Speed_Right_Motor by 1 every time the routine is executed
if (CC_Speed_Right_Motor > Right_Motor_Speed_Prev) { //If the number of loops is larger then the Right_Motor_Speed_Prev variable
CC_Speed_Right_Motor = 0; //Reset the CC_Speed_Right_Motor variable
Right_Motor_Speed_Prev = Right_Motor_Speed; //Load the next Right_Motor_Speed variable
if (Right_Motor_Speed_Prev < 0) { //If the Right_Motor_Speed_Prev is negative
PORTD &= 0b11101111; //Set output D4 low. Reverse the direction
Right_Motor_Speed_Prev *= -1; //Invert the Right_Motor_Speed_Prev variable
}
else PORTD |= 0b00010000; //Set D4 high. Forward direction.
}
else if (CC_Speed_Right_Motor == 1)PORTD |= 0b00100000; //Set output D5 high to create a pulse for the stepper controller
else if (CC_Speed_Right_Motor == 2)PORTD &= 0b11011111; //Set output D5 low because the pulse only has to last for 20us
}//End of timer rroutine
原文出处
https://electronoobs.com/eng_arduino_tut159.php
|
|
楼主
标题置顶
标题高亮
