2-wheel Inverted Pendulum control program
Dependencies: AsyncSerial Lib_DFPlayerMini Lib_MPU9250_SPI mbed
main.cpp@0:c86a79c8b787, 2017-09-15 (annotated)
- Committer:
- bluefish
- Date:
- Fri Sep 15 14:54:59 2017 +0000
- Revision:
- 0:c86a79c8b787
- Child:
- 1:a6cb5f642e69
???????
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
bluefish | 0:c86a79c8b787 | 1 | #include "mbed.h" |
bluefish | 0:c86a79c8b787 | 2 | #include "defines.h" |
bluefish | 0:c86a79c8b787 | 3 | |
bluefish | 0:c86a79c8b787 | 4 | #define K_ANGLE (1000.0f) |
bluefish | 0:c86a79c8b787 | 5 | #define K_ANGLE_VEL (5.0f) |
bluefish | 0:c86a79c8b787 | 6 | |
bluefish | 0:c86a79c8b787 | 7 | STRUCTSENSOR sensor; |
bluefish | 0:c86a79c8b787 | 8 | STRUCTPOSTURE posture; |
bluefish | 0:c86a79c8b787 | 9 | STRUCTCONRTOLPARAM control; |
bluefish | 0:c86a79c8b787 | 10 | |
bluefish | 0:c86a79c8b787 | 11 | |
bluefish | 0:c86a79c8b787 | 12 | |
bluefish | 0:c86a79c8b787 | 13 | // class instance |
bluefish | 0:c86a79c8b787 | 14 | Ticker tic_sen_ctrl; |
bluefish | 0:c86a79c8b787 | 15 | CAN can_driver( CAN_RX, CAN_TX ); |
bluefish | 0:c86a79c8b787 | 16 | MPU9250 imu( MPU9250_CS, MPU9250_MOSI, MPU9250_MISO, MPU9250_SCK ); |
bluefish | 0:c86a79c8b787 | 17 | DFPlayerMini player( DFPLAYER_BUSY, DFPLAYER_TX, DFPLAYER_RX ); |
bluefish | 0:c86a79c8b787 | 18 | |
bluefish | 0:c86a79c8b787 | 19 | void beginMotor(); |
bluefish | 0:c86a79c8b787 | 20 | //void controlMotor( int16_t left, int16_t right ); |
bluefish | 0:c86a79c8b787 | 21 | void sensing_and_control(); |
bluefish | 0:c86a79c8b787 | 22 | |
bluefish | 0:c86a79c8b787 | 23 | int main() { |
bluefish | 0:c86a79c8b787 | 24 | uint8_t isLoop = 0x01; |
bluefish | 0:c86a79c8b787 | 25 | |
bluefish | 0:c86a79c8b787 | 26 | #ifdef USE_SERIAL_DEBUG |
bluefish | 0:c86a79c8b787 | 27 | usb_serial.baud( DEBUG_BAUDRATE ); |
bluefish | 0:c86a79c8b787 | 28 | usb_serial.format( 8, RawSerial::None, 1 ); |
bluefish | 0:c86a79c8b787 | 29 | usb_serial.printf( "USB Serial Debug Enable\n" ); |
bluefish | 0:c86a79c8b787 | 30 | #endif |
bluefish | 0:c86a79c8b787 | 31 | |
bluefish | 0:c86a79c8b787 | 32 | // initialize control gain |
bluefish | 0:c86a79c8b787 | 33 | control.K_angle = K_ANGLE / MPU9250G_DEG2RAD; |
bluefish | 0:c86a79c8b787 | 34 | control.K_angle_vel = K_ANGLE_VEL / MPU9250G_DEG2RAD; |
bluefish | 0:c86a79c8b787 | 35 | |
bluefish | 0:c86a79c8b787 | 36 | // initialize CAN |
bluefish | 0:c86a79c8b787 | 37 | can_driver.frequency( 500000 ); |
bluefish | 0:c86a79c8b787 | 38 | |
bluefish | 0:c86a79c8b787 | 39 | // initialize sensor |
bluefish | 0:c86a79c8b787 | 40 | imu.begin(); |
bluefish | 0:c86a79c8b787 | 41 | imu.setAccelRange( BITS_FS_16G ); |
bluefish | 0:c86a79c8b787 | 42 | imu.setGyroRange( BITS_FS_2000DPS ); |
bluefish | 0:c86a79c8b787 | 43 | |
bluefish | 0:c86a79c8b787 | 44 | // initialize player |
bluefish | 0:c86a79c8b787 | 45 | /* |
bluefish | 0:c86a79c8b787 | 46 | player.begin(); |
bluefish | 0:c86a79c8b787 | 47 | player.volumeSet( 5 ); |
bluefish | 0:c86a79c8b787 | 48 | player.playNumber( 1 ); |
bluefish | 0:c86a79c8b787 | 49 | */ |
bluefish | 0:c86a79c8b787 | 50 | |
bluefish | 0:c86a79c8b787 | 51 | // wait motor driver ON |
bluefish | 0:c86a79c8b787 | 52 | wait( 0.1f ); |
bluefish | 0:c86a79c8b787 | 53 | |
bluefish | 0:c86a79c8b787 | 54 | // move driver to operation |
bluefish | 0:c86a79c8b787 | 55 | beginMotor(); |
bluefish | 0:c86a79c8b787 | 56 | |
bluefish | 0:c86a79c8b787 | 57 | // start ticker interrupt |
bluefish | 0:c86a79c8b787 | 58 | tic_sen_ctrl.attach( sensing_and_control, PROCESS_INTERVAL_SEC ); |
bluefish | 0:c86a79c8b787 | 59 | |
bluefish | 0:c86a79c8b787 | 60 | // main loop |
bluefish | 0:c86a79c8b787 | 61 | while( isLoop ){ |
bluefish | 0:c86a79c8b787 | 62 | } |
bluefish | 0:c86a79c8b787 | 63 | return 0; |
bluefish | 0:c86a79c8b787 | 64 | } |
bluefish | 0:c86a79c8b787 | 65 | |
bluefish | 0:c86a79c8b787 | 66 | void beginMotor(){ |
bluefish | 0:c86a79c8b787 | 67 | CANMessage msg; |
bluefish | 0:c86a79c8b787 | 68 | |
bluefish | 0:c86a79c8b787 | 69 | msg.format = CANStandard; |
bluefish | 0:c86a79c8b787 | 70 | msg.type = CANData; |
bluefish | 0:c86a79c8b787 | 71 | msg.id = 0x410; |
bluefish | 0:c86a79c8b787 | 72 | msg.len = 2; |
bluefish | 0:c86a79c8b787 | 73 | msg.data[0] = DEVID_LEFT; |
bluefish | 0:c86a79c8b787 | 74 | msg.data[1] = 0x06; |
bluefish | 0:c86a79c8b787 | 75 | can_driver.write( msg ); |
bluefish | 0:c86a79c8b787 | 76 | |
bluefish | 0:c86a79c8b787 | 77 | wait( 0.005f ); |
bluefish | 0:c86a79c8b787 | 78 | |
bluefish | 0:c86a79c8b787 | 79 | msg.format = CANStandard; |
bluefish | 0:c86a79c8b787 | 80 | msg.type = CANData; |
bluefish | 0:c86a79c8b787 | 81 | msg.id = 0x410; |
bluefish | 0:c86a79c8b787 | 82 | msg.len = 2; |
bluefish | 0:c86a79c8b787 | 83 | msg.data[0] = DEVID_RIGHT; |
bluefish | 0:c86a79c8b787 | 84 | msg.data[1] = 0x06; |
bluefish | 0:c86a79c8b787 | 85 | can_driver.write( msg ); |
bluefish | 0:c86a79c8b787 | 86 | |
bluefish | 0:c86a79c8b787 | 87 | return; |
bluefish | 0:c86a79c8b787 | 88 | } |
bluefish | 0:c86a79c8b787 | 89 | |
bluefish | 0:c86a79c8b787 | 90 | void controlMotor( int16_t left, int16_t right ){ |
bluefish | 0:c86a79c8b787 | 91 | CANMessage msg; |
bluefish | 0:c86a79c8b787 | 92 | |
bluefish | 0:c86a79c8b787 | 93 | left = -left; |
bluefish | 0:c86a79c8b787 | 94 | msg.format = CANStandard; |
bluefish | 0:c86a79c8b787 | 95 | msg.type = CANData; |
bluefish | 0:c86a79c8b787 | 96 | msg.id = 0x420; |
bluefish | 0:c86a79c8b787 | 97 | msg.len = 8; |
bluefish | 0:c86a79c8b787 | 98 | msg.data[0] = DEVID_LEFT; |
bluefish | 0:c86a79c8b787 | 99 | msg.data[1] = 0x06; |
bluefish | 0:c86a79c8b787 | 100 | msg.data[2] = 0x00; |
bluefish | 0:c86a79c8b787 | 101 | msg.data[3] = 0x00; |
bluefish | 0:c86a79c8b787 | 102 | msg.data[4] = ( left >> 8 ) & 0xFF; |
bluefish | 0:c86a79c8b787 | 103 | msg.data[5] = ( left >> 0 ) & 0xFF; |
bluefish | 0:c86a79c8b787 | 104 | msg.data[6] = 0x00; |
bluefish | 0:c86a79c8b787 | 105 | msg.data[7] = 0x00; |
bluefish | 0:c86a79c8b787 | 106 | can_driver.write( msg ); |
bluefish | 0:c86a79c8b787 | 107 | |
bluefish | 0:c86a79c8b787 | 108 | wait( 0.005f ); |
bluefish | 0:c86a79c8b787 | 109 | |
bluefish | 0:c86a79c8b787 | 110 | right = right; |
bluefish | 0:c86a79c8b787 | 111 | msg.format = CANStandard; |
bluefish | 0:c86a79c8b787 | 112 | msg.type = CANData; |
bluefish | 0:c86a79c8b787 | 113 | msg.id = 0x420; |
bluefish | 0:c86a79c8b787 | 114 | msg.len = 8; |
bluefish | 0:c86a79c8b787 | 115 | msg.data[0] = DEVID_RIGHT; |
bluefish | 0:c86a79c8b787 | 116 | msg.data[1] = 0x06; |
bluefish | 0:c86a79c8b787 | 117 | msg.data[2] = 0x00; |
bluefish | 0:c86a79c8b787 | 118 | msg.data[3] = 0x00; |
bluefish | 0:c86a79c8b787 | 119 | msg.data[4] = ( right >> 8 ) & 0xFF; |
bluefish | 0:c86a79c8b787 | 120 | msg.data[5] = ( right >> 0 ) & 0xFF; |
bluefish | 0:c86a79c8b787 | 121 | msg.data[6] = 0x00; |
bluefish | 0:c86a79c8b787 | 122 | msg.data[7] = 0x00; |
bluefish | 0:c86a79c8b787 | 123 | can_driver.write( msg ); |
bluefish | 0:c86a79c8b787 | 124 | |
bluefish | 0:c86a79c8b787 | 125 | return; |
bluefish | 0:c86a79c8b787 | 126 | } |
bluefish | 0:c86a79c8b787 | 127 | |
bluefish | 0:c86a79c8b787 | 128 | void sensing_and_control(){ |
bluefish | 0:c86a79c8b787 | 129 | //LPF variables |
bluefish | 0:c86a79c8b787 | 130 | int16_t com = 0; |
bluefish | 0:c86a79c8b787 | 131 | static float angle_int = 0.0f; |
bluefish | 0:c86a79c8b787 | 132 | static float angle_adj = 0.0f; |
bluefish | 0:c86a79c8b787 | 133 | static float angle_adj_t = 0.0f; |
bluefish | 0:c86a79c8b787 | 134 | |
bluefish | 0:c86a79c8b787 | 135 | // read sensor |
bluefish | 0:c86a79c8b787 | 136 | imu.read6Axis( |
bluefish | 0:c86a79c8b787 | 137 | &sensor.acc[0], &sensor.acc[1], &sensor.acc[2], |
bluefish | 0:c86a79c8b787 | 138 | &sensor.gyro[0], &sensor.gyro[1], &sensor.gyro[2] |
bluefish | 0:c86a79c8b787 | 139 | ); |
bluefish | 0:c86a79c8b787 | 140 | #ifdef USE_SERIAL_DEBUG |
bluefish | 0:c86a79c8b787 | 141 | usb_serial.printf( "%f\t %f\t %f\t %f\t %f\t %f\n", |
bluefish | 0:c86a79c8b787 | 142 | sensor.acc[0], sensor.acc[1], sensor.acc[2], |
bluefish | 0:c86a79c8b787 | 143 | sensor.gyro[0], sensor.gyro[1], sensor.gyro[2] |
bluefish | 0:c86a79c8b787 | 144 | ); |
bluefish | 0:c86a79c8b787 | 145 | #endif |
bluefish | 0:c86a79c8b787 | 146 | |
bluefish | 0:c86a79c8b787 | 147 | sensor.gyro[0] *= MPU9250G_DEG2RAD; |
bluefish | 0:c86a79c8b787 | 148 | sensor.gyro[1] *= MPU9250G_DEG2RAD; |
bluefish | 0:c86a79c8b787 | 149 | sensor.gyro[2] *= MPU9250G_DEG2RAD; |
bluefish | 0:c86a79c8b787 | 150 | |
bluefish | 0:c86a79c8b787 | 151 | // filter |
bluefish | 0:c86a79c8b787 | 152 | angle_adj_t = angle_adj; |
bluefish | 0:c86a79c8b787 | 153 | angle_int += sensor.gyro[1] * PROCESS_INTERVAL_SEC; // integral gyro |
bluefish | 0:c86a79c8b787 | 154 | angle_adj = -( sensor.acc[0] / sensor.acc[2] ) - angle_int; // |
bluefish | 0:c86a79c8b787 | 155 | angle_adj = angle_adj_t + PROCESS_INTERVAL_SEC * ( angle_adj - angle_adj_t ) / 3.0f; // calc correction via LPF |
bluefish | 0:c86a79c8b787 | 156 | |
bluefish | 0:c86a79c8b787 | 157 | // update state |
bluefish | 0:c86a79c8b787 | 158 | posture.angle = angle_int + angle_adj; |
bluefish | 0:c86a79c8b787 | 159 | posture.angle_vec = sensor.gyro[1]; |
bluefish | 0:c86a79c8b787 | 160 | |
bluefish | 0:c86a79c8b787 | 161 | #ifdef USE_SERIAL_DEBUG |
bluefish | 0:c86a79c8b787 | 162 | usb_serial.printf( "%f\t %f\n", |
bluefish | 0:c86a79c8b787 | 163 | posture.angle / MPU9250G_DEG2RAD, |
bluefish | 0:c86a79c8b787 | 164 | posture.angle_vec / MPU9250G_DEG2RAD |
bluefish | 0:c86a79c8b787 | 165 | ); |
bluefish | 0:c86a79c8b787 | 166 | #endif |
bluefish | 0:c86a79c8b787 | 167 | |
bluefish | 0:c86a79c8b787 | 168 | // control motor |
bluefish | 0:c86a79c8b787 | 169 | com = (int16_t)( |
bluefish | 0:c86a79c8b787 | 170 | posture.angle * control.K_angle |
bluefish | 0:c86a79c8b787 | 171 | + posture.angle_vec * control.K_angle_vel ); |
bluefish | 0:c86a79c8b787 | 172 | controlMotor( com, com ); |
bluefish | 0:c86a79c8b787 | 173 | |
bluefish | 0:c86a79c8b787 | 174 | return; |
bluefish | 0:c86a79c8b787 | 175 | } |