|
|
| 第97行： |
第97行： |
| | '''注意：'''注意图中所示电机接口方向，两个电机接口应该对称安装。 | | '''注意：'''注意图中所示电机接口方向，两个电机接口应该对称安装。 |
| | [[File:Balancestep2-1.jpg||350px|center]] | | [[File:Balancestep2-1.jpg||350px|center]] |
| | + | [[File:BalancestepLine.jpg||350px|center]] |
| | *步骤2完成后组成'''电机组件''' | | *步骤2完成后组成'''电机组件''' |
| | [[File:Balancestep2-2.jpg||350px|center]] | | [[File:Balancestep2-2.jpg||350px|center]] |
| 第179行： |
第180行： |
| | *在平衡车使用过程中发现电机转速比较慢，平衡车无法直立起来，此时是'''2s电池'''电量不足，需要使用配套的锂电池平衡充给电池充电 | | *在平衡车使用过程中发现电机转速比较慢，平衡车无法直立起来，此时是'''2s电池'''电量不足，需要使用配套的锂电池平衡充给电池充电 |
| | *请按下图所示将'''2s电池'''的白色3pin接口插入平衡充的3pin插口内，然后接上平衡充的电源 | | *请按下图所示将'''2s电池'''的白色3pin接口插入平衡充的3pin插口内，然后接上平衡充的电源 |
| | + | [[File:BalanceCharge1.jpg||350px|center]] |
| | + | *可以通过平衡充的充电指示灯判断充电状态，各种状态如下： |
| | + | {|class="wikitable" |
| | + | |- |
| | + | |指示灯状态||充电状态 |
| | + | |- |
| | + | |绿灯闪烁||没有接电池 |
| | + | |- |
| | + | |红灯常亮 ||电池正在充电 |
| | + | |- |
| | + | |绿灯常亮 ||电池已充满 |
| | | | |
| − | *下载程序时候最好只叠加core（core+）和USBTTL，虽然本次搭建涉及的nRF24不会引起冲突，但是别的通信模块有时会造成串口冲突，养成好习惯。
| + | |} |
| − | *Core+要叠在nRF24，USB的底下，紧贴ROBOT板。
| + | ===平衡车无法直立=== |
| − | *锂电池正负极别接错了，否则会烧坏电路。
| + | *请对照如下图判断[[Microduino-Shield Stepper/zh]]模块方向是否安装正确，'''步进电机线'''是否连接正确。 |
| − | *调试好后，实际运行时不要使用USB供电，供电电压不足，请使用电池
| + | [[File:Balancestep7-1.jpg||350px|center]] |
| − | | + | *在使用过程中发现平衡车倒向一边，并听见'''步进电机'''堵转异响，此时可以将平衡车拨倒向另一边，然后平衡车就可以立起来。 |
| − | ==平衡车程序说明==
| |
| − | nrf设置部分
| |
| − | <source lang="cpp">
| |
| − | //nRF==============================
| |
| − | SPI.begin(); //初始化SPI总线
| |
| − | radio.begin();
| |
| − | //此处与Joypad上设定的通道数对应
| |
| − | network.begin(/*channel*/ 70, /*node address*/ this_node);
| |
| − | | |
| − | digitalWrite(LED, HIGH);
| |
| − | Serial.println("===========start===========");
| |
| − | </source>
| |
| − | 端口设定
| |
| − | <source lang = "cpp">
| |
| − | #define MOTOR_EN 4 //PORTB,0
| |
| − | #define MOTOR1_DIR A0 //PORTA,7
| |
| − | #define MOTOR1_STEP 5 //PORTB,1
| |
| − | #define MOTOR2_DIR A1 //PORTA,6
| |
| − | #define MOTOR2_STEP 6 //PORTB,2
| |
| − | #define MOTOR3_DIR A2 //PORTA,5
| |
| − | #define MOTOR3_STEP 7 //PORTB,3
| |
| − | #define MOTOR4_DIR A3 //PORTA,4
| |
| − | #define MOTOR4_STEP 8 //PORTD,6
| |
| − | </source>
| |
| − | 与遥控器进行数据交换的数组
| |
| − | <source lang="cpp">
| |
| − | struct receive_a //接收
| |
| − | {
| |
| − | uint32_t ms;
| |
| − | uint16_t rf_CH0;
| |
| − | uint16_t rf_CH1;
| |
| − | uint16_t rf_CH2;
| |
| − | uint16_t rf_CH3;
| |
| − | uint16_t rf_CH4;
| |
| − | uint16_t rf_CH5;
| |
| − | uint16_t rf_CH6;
| |
| − | uint16_t rf_CH7;
| |
| − | }; | |
| − | </source>
| |
| − | PID控制实现
| |
| − | <source lang="cpp">
| |
| − | // PD的实施。 DT是毫秒
| |
| − | float stabilityPDControl(float DT, float input, float setPoint, float Kp, float Kd)
| |
| − | {
| |
| − | float error;
| |
| − | float output;
| |
| − | error = setPoint - input;
| |
| − | // Kd的两部分实施
| |
| − | //仅使用输入（传感器）的一部分而不是设定值输入输入（T-2）最大的一个
| |
| − | //而第二个使用该设定值，使之更有点侵略性设定点设定点（T-1）
| |
| − | output = Kp * error + (Kd * (setPoint - setPointOld) - Kd * (input - PID_errorOld2)) / DT; // + 错误 - PID_error_Old2
| |
| − | //Serial.print(Kd*(error-PID_errorOld));Serial.print("\t");
| |
| − | PID_errorOld2 = PID_errorOld;
| |
| − | PID_errorOld = input; // 误差为Kd值是唯一的输入组件
| |
| − | setPointOld = setPoint;
| |
| − | return (output);
| |
| − | }
| |
| − | //P控制实现。
| |
| − | float speedPControl(float input, float setPoint, float Kp)
| |
| − | {
| |
| − | float error;
| |
| − | error = setPoint - input;
| |
| − | return (Kp * error);
| |
| − | }
| |
| − | // PI实现。 DT是毫秒
| |
| − | float speedPIControl(float DT, float input, float setPoint, float Kp, float Ki)
| |
| − | {
| |
| − | float error;
| |
| − | float output;
| |
| − | error = setPoint - input;
| |
| − | PID_errorSum += constrain(error, -ITERM_MAX_ERROR, ITERM_MAX_ERROR);
| |
| − | PID_errorSum = constrain(PID_errorSum, -ITERM_MAX, ITERM_MAX);
| |
| − | output = Kp * error + Ki * PID_errorSum * DT * 0.001;
| |
| − | return (output);
| |
| − | }
| |
| − | </source>
| |
| − | PID算法-平衡控制
| |
| − | <source lang="cpp">
| |
| − | void robot()
| |
| − | {
| |
| − | //===============================================================
| |
| − | timer_value = millis();
| |
| − | | |
| − | // 新的DMP定位解决方案
| |
| − | fifoCount = mpu.getFIFOCount();
| |
| − | if (fifoCount >= 18)
| |
| − | {
| |
| − | if (fifoCount > 18) // 如果我们有一个以上的数据包，我们采取简单的路径：丢弃缓冲区
| |
| − | {
| |
| − | mpu.resetFIFO();
| |
| − | return;
| |
| − | }
| |
| − | | |
| − | loop_counter++;
| |
| − | slow_loop_counter++;
| |
| − | dt = (timer_value - timer_old);
| |
| − | timer_old = timer_value;
| |
| − | angle_adjusted_Old = angle_adjusted;
| |
| − | angle_adjusted = dmpGetPhi() + ANGLE_FIX;
| |
| − | Serial.println(angle_adjusted);
| |
| − | mpu.resetFIFO(); // 我们始终复位FIFO
| |
| − | // 我们计算估计机器人的速度
| |
| − | actual_robot_speed_Old = actual_robot_speed;
| |
| − | actual_robot_speed = (speed_m[1] - speed_m[0]) / 2; // 正面：前锋
| |
| − | // 角速度角度调整角度调整旧
| |
| − | int16_t angular_velocity = (angle_adjusted - angle_adjusted_Old) * 90.0;
| |
| − | // 我们利用机器人速度（T-1）或（T-2），以补偿该延迟
| |
| − | int16_t estimated_speed = actual_robot_speed_Old - angular_velocity;
| |
| − | //估计速度估计过滤滤速
| |
| − | estimated_speed_filtered = estimated_speed_filtered * 0.95 + (float)estimated_speed * 0.05;
| |
| − | //目标角速度PIC ONTROL dt的速度估计过滤油门Kp_thr Ki_thr
| |
| − | target_angle = speedPIControl(dt, estimated_speed_filtered, throttle, Kp_thr, Ki_thr);
| |
| − | //有限的输出 目标角度约束目标角度最大目标角度最大目标角度
| |
| − | target_angle = constrain(target_angle, -max_target_angle, max_target_angle);
| |
| − | | |
| − | if (pushUp_counter > 0) // pushUp mode?
| |
| − | target_angle = 10;
| |
| − | | |
| − | //我们整合输出（加速度）
| |
| − | control_output += stabilityPDControl(dt, angle_adjusted, target_angle, Kp, Kd);
| |
| − | control_output = constrain(control_output, -800, 800); // 限制最大输出控制
| |
| − | // 控制的转向部分的输出直接注射
| |
| − | motor1 = control_output + steering;
| |
| − | motor2 = -control_output + steering; //马达2反转
| |
| − | // 限制最大速度
| |
| − | motor1 = constrain(motor1, -500, 500);
| |
| − | motor2 = constrain(motor2, -500, 500);
| |
| − | | |
| − | // Is robot ready (upright?)
| |
| − | if ((angle_adjusted < 66) && (angle_adjusted > -66))
| |
| − | {
| |
| − | if (node_STA) // pushUp mode?
| |
| − | {
| |
| − | pushUp_counter++;
| |
| − | | |
| − | if (pushUp_counter > 60) // 0.3 seconds
| |
| − | {
| |
| − | // Set motors to 0 => disable steppers => robot
| |
| − | setMotorSpeed(0, 0);
| |
| − | setMotorSpeed(1, 0);
| |
| − | // We prepare the raiseup mode
| |
| − | Kp = KP_RAISEUP; // CONTROL GAINS FOR RAISE UP
| |
| − | Kd = KD_RAISEUP;
| |
| − | Kp_thr = KP_THROTTLE_RAISEUP;
| |
| − | control_output = 0;
| |
| − | estimated_speed_filtered = 0;
| |
| − | }
| |
| − | else
| |
| − | {
| |
| − | setMotorSpeed(0, motor1);
| |
| − | setMotorSpeed(1, motor2);
| |
| − | }
| |
| − | }
| |
| − | else
| |
| − | {
| |
| − | // NORMAL MODE
| |
| − | setMotorSpeed(0, motor1);
| |
| − | setMotorSpeed(1, motor2);
| |
| − | pushUp_counter = 0;
| |
| − | }
| |
| − | | |
| − | if ((angle_adjusted < 40) && (angle_adjusted > -40))
| |
| − | {
| |
| − | Kp = Kp_user; // Default or user control gains
| |
| − | Kd = Kd_user;
| |
| − | Kp_thr = Kp_thr_user;
| |
| − | Ki_thr = Ki_thr_user;
| |
| − | }
| |
| − | else
| |
| − | {
| |
| − | Kp = KP_RAISEUP; // CONTROL GAINS FOR RAISE UP
| |
| − | Kd = KD_RAISEUP;
| |
| − | Kp_thr = KP_THROTTLE_RAISEUP;
| |
| − | Ki_thr = KI_THROTTLE_RAISEUP;
| |
| − | }
| |
| − | }
| |
| − | else // Robot not ready, angle > 70º
| |
| − | {
| |
| − | setMotorSpeed(0, 0);
| |
| − | setMotorSpeed(1, 0);
| |
| − | PID_errorSum = 0; // Reset PID I term
| |
| − | Kp = KP_RAISEUP; // CONTROL GAINS FOR RAISE UP
| |
| − | Kd = KD_RAISEUP;
| |
| − | Kp_thr = KP_THROTTLE_RAISEUP;
| |
| − | Ki_thr = KI_THROTTLE_RAISEUP;
| |
| − | }
| |
| − | | |
| − | }
| |
| − | }
| |
| − | </source>
| |
| − | 遥控器数据接收后选择数据控制平衡车的前后与左右
| |
| − | <source lang="cpp">
| |
| − | float * _i = _speed;
| |
| − | //CH1,CH0,CH7对应之前的发送/接收部分定义的数组
| |
| − | _i[0] = map(rec.rf_CH1, 1000, 2000, -MAX_THROTTLE, MAX_THROTTLE);
| |
| − | _i = _turn;
| |
| − | _i[0] = map(rec.rf_CH0, 1000, 2000, -MAX_STEERING, MAX_STEERING);
| |
| − | | |
| − | node_STA = (rec.rf_CH7 > 1500 ? true : false); //接收请求时序赋值
| |
| − | </source>
| |
| − | | |
| − | ==Joypad程序及说明==
| |
| − | Joypad部分
| |
| − | def.h中
| |
| − | 定义了
| |
| − | <source lang = "cpp">
| |
| − | uint8_t nrf_channal = 70; //0~125
| |
| − | </source>
| |
| − | nrf_channal为nrf通信的通道，joypad和Cube小车的代码中都会有该定义
| |
| − | 当通道一致时则Joypad可与Cube小车成功连接。
| |
| − | 在小车代码中会有如下程序段
| |
| − | <source lang = "cpp">
| |
| − | //nRF==============================
| |
| − | SPI.begin(); //初始化SPI总线
| |
| − | radio.begin();
| |
| − | network.begin(/*channel*/ 70 , /*node address*/ this_node);
| |
| − | </source>
| |
| − | 在data.h中
| |
| − | <source lang = "cpp">
| |
| − | outBuf[0] = Joy1_x;
| |
| − | outBuf[1] = Joy1_y;
| |
| − | outBuf[2] = Joy_x;
| |
| − | outBuf[3] = Joy_y;
| |
| − | outBuf[4] = map(AUX[0], 0, 1, Joy_MID - Joy_maximum, Joy_MID + Joy_maximum);
| |
| − | outBuf[5] = map(AUX[1], 0, 1, Joy_MID - Joy_maximum, Joy_MID + Joy_maximum);
| |
| − | outBuf[6] = map(AUX[2], 0, 1, Joy_MID - Joy_maximum, Joy_MID + Joy_maximum);
| |
| − | outBuf[7] = map(AUX[3], 0, 1, Joy_MID - Joy_maximum, Joy_MID + Joy_maximum);
| |
| − | </source>
| |
| − | 8位数组outBuf表示Joypad发出的8位数据，0位为右摇杆左右，1为右摇杆上下，2为左摇杆左右，3位左摇杆上下,4~7位对应AUX0~4
| |
| − | | |
| − | 在nrf.h中
| |
| − | <source lang = "cpp">
| |
| − | struct send_a //发送
| |
| − | {
| |
| − | uint32_t ms;
| |
| − | uint16_t rf_CH0;
| |
| − | uint16_t rf_CH1;
| |
| − | uint16_t rf_CH2;
| |
| − | uint16_t rf_CH3;
| |
| − | uint16_t rf_CH4;
| |
| − | uint16_t rf_CH5;
| |
| − | uint16_t rf_CH6;
| |
| − | uint16_t rf_CH7;
| |
| − | };
| |
| − | </source>
| |
| − | 此处定义的send_a结构体位对应的0位要发送的数据
| |
| | | | |
| | ==视频== | | ==视频== |
