2-wheel Inverted Pendulum control program
Dependencies: AsyncSerial Lib_DFPlayerMini Lib_MPU9250_SPI mbed
main.cpp@4:72c26c07c251, 2017-11-20 (annotated)
- Committer:
- bluefish
- Date:
- Mon Nov 20 00:43:51 2017 +0000
- Revision:
- 4:72c26c07c251
- Parent:
- 2:9c83de2d33ca
- Child:
- 5:f3cbcb294ba9
???????
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 | 1:a6cb5f642e69 | 4 | #define FILESYSTEM_PATH "local" |
bluefish | 1:a6cb5f642e69 | 5 | #define FILESYSTEM_GAIN "/local/gain.txt" |
bluefish | 1:a6cb5f642e69 | 6 | |
bluefish | 0:c86a79c8b787 | 7 | #define K_ANGLE (1000.0f) |
bluefish | 0:c86a79c8b787 | 8 | #define K_ANGLE_VEL (5.0f) |
bluefish | 0:c86a79c8b787 | 9 | |
bluefish | 0:c86a79c8b787 | 10 | STRUCTSENSOR sensor; |
bluefish | 0:c86a79c8b787 | 11 | STRUCTPOSTURE posture; |
bluefish | 0:c86a79c8b787 | 12 | STRUCTCONRTOLPARAM control; |
bluefish | 2:9c83de2d33ca | 13 | STRUCTPAD pad; |
bluefish | 2:9c83de2d33ca | 14 | |
bluefish | 0:c86a79c8b787 | 15 | |
bluefish | 0:c86a79c8b787 | 16 | // class instance |
bluefish | 0:c86a79c8b787 | 17 | Ticker tic_sen_ctrl; |
bluefish | 0:c86a79c8b787 | 18 | CAN can_driver( CAN_RX, CAN_TX ); |
bluefish | 0:c86a79c8b787 | 19 | MPU9250 imu( MPU9250_CS, MPU9250_MOSI, MPU9250_MISO, MPU9250_SCK ); |
bluefish | 4:72c26c07c251 | 20 | //AsyncSerial bt( BLUETOOTH_TX, BLUETOOTH_RX ); |
bluefish | 2:9c83de2d33ca | 21 | //DFPlayerMini player( DFPLAYER_BUSY, DFPLAYER_TX, DFPLAYER_RX ); |
bluefish | 4:72c26c07c251 | 22 | LocalFileSystem file_local( FILESYSTEM_PATH ); |
bluefish | 0:c86a79c8b787 | 23 | |
bluefish | 0:c86a79c8b787 | 24 | void beginMotor(); |
bluefish | 4:72c26c07c251 | 25 | void controlMotor( int16_t left, int16_t right ); |
bluefish | 0:c86a79c8b787 | 26 | void sensing_and_control(); |
bluefish | 4:72c26c07c251 | 27 | //void receive_command(); |
bluefish | 2:9c83de2d33ca | 28 | |
bluefish | 2:9c83de2d33ca | 29 | uint8_t cnt = 0; |
bluefish | 0:c86a79c8b787 | 30 | |
bluefish | 0:c86a79c8b787 | 31 | int main() { |
bluefish | 0:c86a79c8b787 | 32 | uint8_t isLoop = 0x01; |
bluefish | 1:a6cb5f642e69 | 33 | FILE* fp; |
bluefish | 0:c86a79c8b787 | 34 | |
bluefish | 0:c86a79c8b787 | 35 | #ifdef USE_SERIAL_DEBUG |
bluefish | 0:c86a79c8b787 | 36 | usb_serial.baud( DEBUG_BAUDRATE ); |
bluefish | 0:c86a79c8b787 | 37 | usb_serial.format( 8, RawSerial::None, 1 ); |
bluefish | 0:c86a79c8b787 | 38 | usb_serial.printf( "USB Serial Debug Enable\n" ); |
bluefish | 0:c86a79c8b787 | 39 | #endif |
bluefish | 0:c86a79c8b787 | 40 | |
bluefish | 0:c86a79c8b787 | 41 | // initialize control gain |
bluefish | 1:a6cb5f642e69 | 42 | fp = fopen( FILESYSTEM_GAIN, "r" ); |
bluefish | 1:a6cb5f642e69 | 43 | fscanf( fp, "%f\r\n", &control.K_angle ); |
bluefish | 1:a6cb5f642e69 | 44 | fscanf( fp, "%f\r\n", &control.K_angle_vel ); |
bluefish | 1:a6cb5f642e69 | 45 | fscanf( fp, "%f\r\n", &control.K_wheel ); |
bluefish | 1:a6cb5f642e69 | 46 | fscanf( fp, "%f\r\n", &control.K_wheel_vel ); |
bluefish | 1:a6cb5f642e69 | 47 | fscanf( fp, "%f\r\n", &control.K_rot_vel ); |
bluefish | 4:72c26c07c251 | 48 | fscanf( fp, "%f\r\n", &control.K_trans_vel ); |
bluefish | 4:72c26c07c251 | 49 | fscanf( fp, "%f\r\n", &control.C_max_angle ); |
bluefish | 1:a6cb5f642e69 | 50 | fclose( fp ); |
bluefish | 1:a6cb5f642e69 | 51 | |
bluefish | 4:72c26c07c251 | 52 | // coefficient |
bluefish | 1:a6cb5f642e69 | 53 | control.K_angle /= MPU9250G_DEG2RAD; |
bluefish | 1:a6cb5f642e69 | 54 | control.K_angle_vel /= MPU9250G_DEG2RAD; |
bluefish | 4:72c26c07c251 | 55 | control.C_max_angle /= MPU9250G_DEG2RAD; |
bluefish | 0:c86a79c8b787 | 56 | |
bluefish | 0:c86a79c8b787 | 57 | // initialize CAN |
bluefish | 0:c86a79c8b787 | 58 | can_driver.frequency( 500000 ); |
bluefish | 0:c86a79c8b787 | 59 | |
bluefish | 0:c86a79c8b787 | 60 | // initialize sensor |
bluefish | 0:c86a79c8b787 | 61 | imu.begin(); |
bluefish | 0:c86a79c8b787 | 62 | imu.setAccelRange( BITS_FS_16G ); |
bluefish | 0:c86a79c8b787 | 63 | imu.setGyroRange( BITS_FS_2000DPS ); |
bluefish | 0:c86a79c8b787 | 64 | |
bluefish | 4:72c26c07c251 | 65 | // initialize UART for wireless |
bluefish | 4:72c26c07c251 | 66 | /* |
bluefish | 2:9c83de2d33ca | 67 | bt.baud( 115200 ); |
bluefish | 2:9c83de2d33ca | 68 | bt.format( 8, RawSerial::None, 1 ); |
bluefish | 4:72c26c07c251 | 69 | */ |
bluefish | 2:9c83de2d33ca | 70 | |
bluefish | 0:c86a79c8b787 | 71 | // initialize player |
bluefish | 0:c86a79c8b787 | 72 | /* |
bluefish | 0:c86a79c8b787 | 73 | player.begin(); |
bluefish | 0:c86a79c8b787 | 74 | player.volumeSet( 5 ); |
bluefish | 0:c86a79c8b787 | 75 | player.playNumber( 1 ); |
bluefish | 0:c86a79c8b787 | 76 | */ |
bluefish | 0:c86a79c8b787 | 77 | |
bluefish | 0:c86a79c8b787 | 78 | // wait motor driver ON |
bluefish | 0:c86a79c8b787 | 79 | wait( 0.1f ); |
bluefish | 0:c86a79c8b787 | 80 | |
bluefish | 0:c86a79c8b787 | 81 | // move driver to operation |
bluefish | 0:c86a79c8b787 | 82 | beginMotor(); |
bluefish | 0:c86a79c8b787 | 83 | |
bluefish | 0:c86a79c8b787 | 84 | // start ticker interrupt |
bluefish | 0:c86a79c8b787 | 85 | tic_sen_ctrl.attach( sensing_and_control, PROCESS_INTERVAL_SEC ); |
bluefish | 0:c86a79c8b787 | 86 | |
bluefish | 0:c86a79c8b787 | 87 | // main loop |
bluefish | 0:c86a79c8b787 | 88 | while( isLoop ){ |
bluefish | 0:c86a79c8b787 | 89 | } |
bluefish | 0:c86a79c8b787 | 90 | return 0; |
bluefish | 0:c86a79c8b787 | 91 | } |
bluefish | 0:c86a79c8b787 | 92 | |
bluefish | 0:c86a79c8b787 | 93 | void beginMotor(){ |
bluefish | 0:c86a79c8b787 | 94 | CANMessage msg; |
bluefish | 0:c86a79c8b787 | 95 | |
bluefish | 0:c86a79c8b787 | 96 | msg.format = CANStandard; |
bluefish | 0:c86a79c8b787 | 97 | msg.type = CANData; |
bluefish | 0:c86a79c8b787 | 98 | msg.id = 0x410; |
bluefish | 0:c86a79c8b787 | 99 | msg.len = 2; |
bluefish | 0:c86a79c8b787 | 100 | msg.data[0] = DEVID_LEFT; |
bluefish | 0:c86a79c8b787 | 101 | msg.data[1] = 0x06; |
bluefish | 0:c86a79c8b787 | 102 | can_driver.write( msg ); |
bluefish | 0:c86a79c8b787 | 103 | |
bluefish | 0:c86a79c8b787 | 104 | msg.format = CANStandard; |
bluefish | 0:c86a79c8b787 | 105 | msg.type = CANData; |
bluefish | 0:c86a79c8b787 | 106 | msg.id = 0x410; |
bluefish | 0:c86a79c8b787 | 107 | msg.len = 2; |
bluefish | 0:c86a79c8b787 | 108 | msg.data[0] = DEVID_RIGHT; |
bluefish | 0:c86a79c8b787 | 109 | msg.data[1] = 0x06; |
bluefish | 0:c86a79c8b787 | 110 | can_driver.write( msg ); |
bluefish | 0:c86a79c8b787 | 111 | |
bluefish | 0:c86a79c8b787 | 112 | return; |
bluefish | 0:c86a79c8b787 | 113 | } |
bluefish | 0:c86a79c8b787 | 114 | |
bluefish | 0:c86a79c8b787 | 115 | void controlMotor( int16_t left, int16_t right ){ |
bluefish | 0:c86a79c8b787 | 116 | CANMessage msg; |
bluefish | 0:c86a79c8b787 | 117 | |
bluefish | 0:c86a79c8b787 | 118 | left = -left; |
bluefish | 0:c86a79c8b787 | 119 | msg.format = CANStandard; |
bluefish | 0:c86a79c8b787 | 120 | msg.type = CANData; |
bluefish | 0:c86a79c8b787 | 121 | msg.id = 0x420; |
bluefish | 0:c86a79c8b787 | 122 | msg.len = 8; |
bluefish | 0:c86a79c8b787 | 123 | msg.data[0] = DEVID_LEFT; |
bluefish | 0:c86a79c8b787 | 124 | msg.data[1] = 0x06; |
bluefish | 0:c86a79c8b787 | 125 | msg.data[2] = 0x00; |
bluefish | 0:c86a79c8b787 | 126 | msg.data[3] = 0x00; |
bluefish | 0:c86a79c8b787 | 127 | msg.data[4] = ( left >> 8 ) & 0xFF; |
bluefish | 0:c86a79c8b787 | 128 | msg.data[5] = ( left >> 0 ) & 0xFF; |
bluefish | 0:c86a79c8b787 | 129 | msg.data[6] = 0x00; |
bluefish | 0:c86a79c8b787 | 130 | msg.data[7] = 0x00; |
bluefish | 0:c86a79c8b787 | 131 | can_driver.write( msg ); |
bluefish | 0:c86a79c8b787 | 132 | |
bluefish | 0:c86a79c8b787 | 133 | right = right; |
bluefish | 0:c86a79c8b787 | 134 | msg.format = CANStandard; |
bluefish | 0:c86a79c8b787 | 135 | msg.type = CANData; |
bluefish | 0:c86a79c8b787 | 136 | msg.id = 0x420; |
bluefish | 0:c86a79c8b787 | 137 | msg.len = 8; |
bluefish | 0:c86a79c8b787 | 138 | msg.data[0] = DEVID_RIGHT; |
bluefish | 0:c86a79c8b787 | 139 | msg.data[1] = 0x06; |
bluefish | 0:c86a79c8b787 | 140 | msg.data[2] = 0x00; |
bluefish | 0:c86a79c8b787 | 141 | msg.data[3] = 0x00; |
bluefish | 0:c86a79c8b787 | 142 | msg.data[4] = ( right >> 8 ) & 0xFF; |
bluefish | 0:c86a79c8b787 | 143 | msg.data[5] = ( right >> 0 ) & 0xFF; |
bluefish | 0:c86a79c8b787 | 144 | msg.data[6] = 0x00; |
bluefish | 0:c86a79c8b787 | 145 | msg.data[7] = 0x00; |
bluefish | 0:c86a79c8b787 | 146 | can_driver.write( msg ); |
bluefish | 0:c86a79c8b787 | 147 | |
bluefish | 0:c86a79c8b787 | 148 | return; |
bluefish | 0:c86a79c8b787 | 149 | } |
bluefish | 0:c86a79c8b787 | 150 | |
bluefish | 0:c86a79c8b787 | 151 | void sensing_and_control(){ |
bluefish | 0:c86a79c8b787 | 152 | //LPF variables |
bluefish | 2:9c83de2d33ca | 153 | int16_t com = 0; |
bluefish | 2:9c83de2d33ca | 154 | int16_t comL = 0; |
bluefish | 4:72c26c07c251 | 155 | int16_t comR = 0; |
bluefish | 0:c86a79c8b787 | 156 | static float angle_int = 0.0f; |
bluefish | 0:c86a79c8b787 | 157 | static float angle_adj = 0.0f; |
bluefish | 0:c86a79c8b787 | 158 | static float angle_adj_t = 0.0f; |
bluefish | 0:c86a79c8b787 | 159 | |
bluefish | 0:c86a79c8b787 | 160 | // read sensor |
bluefish | 0:c86a79c8b787 | 161 | imu.read6Axis( |
bluefish | 0:c86a79c8b787 | 162 | &sensor.acc[0], &sensor.acc[1], &sensor.acc[2], |
bluefish | 0:c86a79c8b787 | 163 | &sensor.gyro[0], &sensor.gyro[1], &sensor.gyro[2] |
bluefish | 0:c86a79c8b787 | 164 | ); |
bluefish | 0:c86a79c8b787 | 165 | #ifdef USE_SERIAL_DEBUG |
bluefish | 0:c86a79c8b787 | 166 | usb_serial.printf( "%f\t %f\t %f\t %f\t %f\t %f\n", |
bluefish | 0:c86a79c8b787 | 167 | sensor.acc[0], sensor.acc[1], sensor.acc[2], |
bluefish | 0:c86a79c8b787 | 168 | sensor.gyro[0], sensor.gyro[1], sensor.gyro[2] |
bluefish | 0:c86a79c8b787 | 169 | ); |
bluefish | 0:c86a79c8b787 | 170 | #endif |
bluefish | 0:c86a79c8b787 | 171 | |
bluefish | 0:c86a79c8b787 | 172 | sensor.gyro[0] *= MPU9250G_DEG2RAD; |
bluefish | 0:c86a79c8b787 | 173 | sensor.gyro[1] *= MPU9250G_DEG2RAD; |
bluefish | 0:c86a79c8b787 | 174 | sensor.gyro[2] *= MPU9250G_DEG2RAD; |
bluefish | 0:c86a79c8b787 | 175 | |
bluefish | 0:c86a79c8b787 | 176 | // filter |
bluefish | 0:c86a79c8b787 | 177 | angle_adj_t = angle_adj; |
bluefish | 0:c86a79c8b787 | 178 | angle_int += sensor.gyro[1] * PROCESS_INTERVAL_SEC; // integral gyro |
bluefish | 0:c86a79c8b787 | 179 | angle_adj = -( sensor.acc[0] / sensor.acc[2] ) - angle_int; // |
bluefish | 0:c86a79c8b787 | 180 | angle_adj = angle_adj_t + PROCESS_INTERVAL_SEC * ( angle_adj - angle_adj_t ) / 3.0f; // calc correction via LPF |
bluefish | 4:72c26c07c251 | 181 | |
bluefish | 0:c86a79c8b787 | 182 | // update state |
bluefish | 0:c86a79c8b787 | 183 | posture.angle = angle_int + angle_adj; |
bluefish | 2:9c83de2d33ca | 184 | posture.angle_vel = sensor.gyro[1]; |
bluefish | 2:9c83de2d33ca | 185 | |
bluefish | 2:9c83de2d33ca | 186 | if( cnt > 0 ){ cnt--; } |
bluefish | 0:c86a79c8b787 | 187 | |
bluefish | 0:c86a79c8b787 | 188 | #ifdef USE_SERIAL_DEBUG |
bluefish | 0:c86a79c8b787 | 189 | usb_serial.printf( "%f\t %f\n", |
bluefish | 0:c86a79c8b787 | 190 | posture.angle / MPU9250G_DEG2RAD, |
bluefish | 0:c86a79c8b787 | 191 | posture.angle_vec / MPU9250G_DEG2RAD |
bluefish | 0:c86a79c8b787 | 192 | ); |
bluefish | 2:9c83de2d33ca | 193 | #endif |
bluefish | 0:c86a79c8b787 | 194 | |
bluefish | 0:c86a79c8b787 | 195 | // control motor |
bluefish | 2:9c83de2d33ca | 196 | com = 0; |
bluefish | 2:9c83de2d33ca | 197 | com += control.K_angle * posture.angle; |
bluefish | 2:9c83de2d33ca | 198 | com += control.K_angle_vel * posture.angle_vel; |
bluefish | 2:9c83de2d33ca | 199 | |
bluefish | 2:9c83de2d33ca | 200 | comL = com; |
bluefish | 2:9c83de2d33ca | 201 | comL += control.K_wheel * posture.wheel[0]; |
bluefish | 2:9c83de2d33ca | 202 | comL += control.K_wheel_vel * posture.wheel[0]; |
bluefish | 2:9c83de2d33ca | 203 | if( pad.enable && cnt > 0 ){ |
bluefish | 2:9c83de2d33ca | 204 | comL += control.K_trans_vel * pad.y; |
bluefish | 2:9c83de2d33ca | 205 | comL += control.K_rot_vel * pad.x; |
bluefish | 2:9c83de2d33ca | 206 | } |
bluefish | 2:9c83de2d33ca | 207 | |
bluefish | 2:9c83de2d33ca | 208 | comR = com; |
bluefish | 2:9c83de2d33ca | 209 | comR += control.K_wheel * posture.wheel[1]; |
bluefish | 2:9c83de2d33ca | 210 | comR += control.K_wheel_vel * posture.wheel[1]; |
bluefish | 2:9c83de2d33ca | 211 | if( pad.enable && cnt > 0 ){ |
bluefish | 2:9c83de2d33ca | 212 | comR += control.K_trans_vel * pad.y; |
bluefish | 2:9c83de2d33ca | 213 | comR -= control.K_rot_vel * pad.x; |
bluefish | 2:9c83de2d33ca | 214 | } |
bluefish | 2:9c83de2d33ca | 215 | |
bluefish | 4:72c26c07c251 | 216 | if( posture.angle < control.C_max_angle && posture.angle > -control.C_max_angle ){ |
bluefish | 2:9c83de2d33ca | 217 | controlMotor( comL, comR ); |
bluefish | 2:9c83de2d33ca | 218 | }else{ |
bluefish | 2:9c83de2d33ca | 219 | controlMotor( 0, 0 ); |
bluefish | 2:9c83de2d33ca | 220 | } |
bluefish | 2:9c83de2d33ca | 221 | |
bluefish | 2:9c83de2d33ca | 222 | receive_command(); |
bluefish | 0:c86a79c8b787 | 223 | |
bluefish | 0:c86a79c8b787 | 224 | return; |
bluefish | 0:c86a79c8b787 | 225 | } |
bluefish | 2:9c83de2d33ca | 226 | |
bluefish | 4:72c26c07c251 | 227 | // receive command function |
bluefish | 4:72c26c07c251 | 228 | /* |
bluefish | 2:9c83de2d33ca | 229 | void receive_command(){ |
bluefish | 2:9c83de2d33ca | 230 | uint8_t c = 0; |
bluefish | 2:9c83de2d33ca | 231 | if( bt.readable() ){ |
bluefish | 2:9c83de2d33ca | 232 | c = bt.getc(); |
bluefish | 2:9c83de2d33ca | 233 | |
bluefish | 2:9c83de2d33ca | 234 | switch( c ){ |
bluefish | 2:9c83de2d33ca | 235 | case 'L': |
bluefish | 2:9c83de2d33ca | 236 | pad.x = -1.0f; |
bluefish | 2:9c83de2d33ca | 237 | cnt = 100; |
bluefish | 2:9c83de2d33ca | 238 | break; |
bluefish | 2:9c83de2d33ca | 239 | case 'R': |
bluefish | 2:9c83de2d33ca | 240 | pad.x = 1.0f; |
bluefish | 2:9c83de2d33ca | 241 | cnt = 100; |
bluefish | 2:9c83de2d33ca | 242 | break; |
bluefish | 2:9c83de2d33ca | 243 | case 'S': |
bluefish | 2:9c83de2d33ca | 244 | pad.x = 0.0f; |
bluefish | 2:9c83de2d33ca | 245 | pad.y = 0.0f; |
bluefish | 2:9c83de2d33ca | 246 | cnt = 100; |
bluefish | 2:9c83de2d33ca | 247 | break; |
bluefish | 2:9c83de2d33ca | 248 | case 'I': |
bluefish | 2:9c83de2d33ca | 249 | pad.enable = 0x01; |
bluefish | 2:9c83de2d33ca | 250 | cnt = 100; |
bluefish | 2:9c83de2d33ca | 251 | break; |
bluefish | 2:9c83de2d33ca | 252 | case 'O': |
bluefish | 2:9c83de2d33ca | 253 | pad.enable = 0x00; |
bluefish | 2:9c83de2d33ca | 254 | cnt = 100; |
bluefish | 2:9c83de2d33ca | 255 | break; |
bluefish | 2:9c83de2d33ca | 256 | } |
bluefish | 2:9c83de2d33ca | 257 | } |
bluefish | 2:9c83de2d33ca | 258 | return; |
bluefish | 2:9c83de2d33ca | 259 | } |
bluefish | 4:72c26c07c251 | 260 | */ |