Shimon AJISAKA / Mbed 2 deprecated InvertedPendulum2017

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?

UserRevisionLine numberNew 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 */