Shimon AJISAKA / Mbed 2 deprecated InvertedPendulum2017

2-wheel Inverted Pendulum control program

Dependencies:   AsyncSerial Lib_DFPlayerMini Lib_MPU9250_SPI mbed

main.cpp@2:9c83de2d33ca, 2017-09-16 (annotated)

Committer:
bluefish
Date:
Sat Sep 16 17:15:30 2017 +0000
Revision:
2:9c83de2d33ca
Parent:
1:a6cb5f642e69
Child:
4:72c26c07c251
?????????????????????????

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 2:9c83de2d33ca 20 AsyncSerial bt( BLUETOOTH_TX, BLUETOOTH_RX );
bluefish 2:9c83de2d33ca 21 //DFPlayerMini player( DFPLAYER_BUSY, DFPLAYER_TX, DFPLAYER_RX );
bluefish 1:a6cb5f642e69 22 LocalFileSystem file_local( FILESYSTEM_PATH );
bluefish 0:c86a79c8b787 23
bluefish 0:c86a79c8b787 24 void beginMotor();
bluefish 0:c86a79c8b787 25 //void controlMotor( int16_t left, int16_t right );
bluefish 0:c86a79c8b787 26 void sensing_and_control();
bluefish 2:9c83de2d33ca 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 1:a6cb5f642e69 48 fscanf( fp, "%f\r\n", &control.K_trans_vel );
bluefish 1:a6cb5f642e69 49 fclose( fp );
bluefish 1:a6cb5f642e69 50
bluefish 1:a6cb5f642e69 51 control.K_angle /= MPU9250G_DEG2RAD;
bluefish 1:a6cb5f642e69 52 control.K_angle_vel /= MPU9250G_DEG2RAD;
bluefish 0:c86a79c8b787 53
bluefish 0:c86a79c8b787 54 // initialize CAN
bluefish 0:c86a79c8b787 55 can_driver.frequency( 500000 );
bluefish 0:c86a79c8b787 56
bluefish 0:c86a79c8b787 57 // initialize sensor
bluefish 0:c86a79c8b787 58 imu.begin();
bluefish 0:c86a79c8b787 59 imu.setAccelRange( BITS_FS_16G );
bluefish 0:c86a79c8b787 60 imu.setGyroRange( BITS_FS_2000DPS );
bluefish 0:c86a79c8b787 61
bluefish 2:9c83de2d33ca 62 bt.baud( 115200 );
bluefish 2:9c83de2d33ca 63 bt.format( 8, RawSerial::None, 1 );
bluefish 2:9c83de2d33ca 64
bluefish 0:c86a79c8b787 65 // initialize player
bluefish 0:c86a79c8b787 66 /*
bluefish 0:c86a79c8b787 67 player.begin();
bluefish 0:c86a79c8b787 68 player.volumeSet( 5 );
bluefish 0:c86a79c8b787 69 player.playNumber( 1 );
bluefish 0:c86a79c8b787 70 */
bluefish 0:c86a79c8b787 71
bluefish 0:c86a79c8b787 72 // wait motor driver ON
bluefish 0:c86a79c8b787 73 wait( 0.1f );
bluefish 0:c86a79c8b787 74
bluefish 0:c86a79c8b787 75 // move driver to operation
bluefish 0:c86a79c8b787 76 beginMotor();
bluefish 0:c86a79c8b787 77
bluefish 0:c86a79c8b787 78 // start ticker interrupt
bluefish 0:c86a79c8b787 79 tic_sen_ctrl.attach( sensing_and_control, PROCESS_INTERVAL_SEC );
bluefish 0:c86a79c8b787 80
bluefish 0:c86a79c8b787 81 // main loop
bluefish 0:c86a79c8b787 82 while( isLoop ){
bluefish 0:c86a79c8b787 83 }
bluefish 0:c86a79c8b787 84 return 0;
bluefish 0:c86a79c8b787 85 }
bluefish 0:c86a79c8b787 86
bluefish 0:c86a79c8b787 87 void beginMotor(){
bluefish 0:c86a79c8b787 88 CANMessage msg;
bluefish 0:c86a79c8b787 89
bluefish 0:c86a79c8b787 90 msg.format = CANStandard;
bluefish 0:c86a79c8b787 91 msg.type = CANData;
bluefish 0:c86a79c8b787 92 msg.id = 0x410;
bluefish 0:c86a79c8b787 93 msg.len = 2;
bluefish 0:c86a79c8b787 94 msg.data[0] = DEVID_LEFT;
bluefish 0:c86a79c8b787 95 msg.data[1] = 0x06;
bluefish 0:c86a79c8b787 96 can_driver.write( msg );
bluefish 0:c86a79c8b787 97
bluefish 0:c86a79c8b787 98 wait( 0.005f );
bluefish 0:c86a79c8b787 99
bluefish 0:c86a79c8b787 100 msg.format = CANStandard;
bluefish 0:c86a79c8b787 101 msg.type = CANData;
bluefish 0:c86a79c8b787 102 msg.id = 0x410;
bluefish 0:c86a79c8b787 103 msg.len = 2;
bluefish 0:c86a79c8b787 104 msg.data[0] = DEVID_RIGHT;
bluefish 0:c86a79c8b787 105 msg.data[1] = 0x06;
bluefish 0:c86a79c8b787 106 can_driver.write( msg );
bluefish 0:c86a79c8b787 107
bluefish 0:c86a79c8b787 108 return;
bluefish 0:c86a79c8b787 109 }
bluefish 0:c86a79c8b787 110
bluefish 0:c86a79c8b787 111 void controlMotor( int16_t left, int16_t right ){
bluefish 0:c86a79c8b787 112 CANMessage msg;
bluefish 0:c86a79c8b787 113
bluefish 0:c86a79c8b787 114 left = -left;
bluefish 0:c86a79c8b787 115 msg.format = CANStandard;
bluefish 0:c86a79c8b787 116 msg.type = CANData;
bluefish 0:c86a79c8b787 117 msg.id = 0x420;
bluefish 0:c86a79c8b787 118 msg.len = 8;
bluefish 0:c86a79c8b787 119 msg.data[0] = DEVID_LEFT;
bluefish 0:c86a79c8b787 120 msg.data[1] = 0x06;
bluefish 0:c86a79c8b787 121 msg.data[2] = 0x00;
bluefish 0:c86a79c8b787 122 msg.data[3] = 0x00;
bluefish 0:c86a79c8b787 123 msg.data[4] = ( left >> 8 ) & 0xFF;
bluefish 0:c86a79c8b787 124 msg.data[5] = ( left >> 0 ) & 0xFF;
bluefish 0:c86a79c8b787 125 msg.data[6] = 0x00;
bluefish 0:c86a79c8b787 126 msg.data[7] = 0x00;
bluefish 0:c86a79c8b787 127 can_driver.write( msg );
bluefish 0:c86a79c8b787 128
bluefish 0:c86a79c8b787 129 wait( 0.005f );
bluefish 0:c86a79c8b787 130
bluefish 0:c86a79c8b787 131 right = right;
bluefish 0:c86a79c8b787 132 msg.format = CANStandard;
bluefish 0:c86a79c8b787 133 msg.type = CANData;
bluefish 0:c86a79c8b787 134 msg.id = 0x420;
bluefish 0:c86a79c8b787 135 msg.len = 8;
bluefish 0:c86a79c8b787 136 msg.data[0] = DEVID_RIGHT;
bluefish 0:c86a79c8b787 137 msg.data[1] = 0x06;
bluefish 0:c86a79c8b787 138 msg.data[2] = 0x00;
bluefish 0:c86a79c8b787 139 msg.data[3] = 0x00;
bluefish 0:c86a79c8b787 140 msg.data[4] = ( right >> 8 ) & 0xFF;
bluefish 0:c86a79c8b787 141 msg.data[5] = ( right >> 0 ) & 0xFF;
bluefish 0:c86a79c8b787 142 msg.data[6] = 0x00;
bluefish 0:c86a79c8b787 143 msg.data[7] = 0x00;
bluefish 0:c86a79c8b787 144 can_driver.write( msg );
bluefish 0:c86a79c8b787 145
bluefish 0:c86a79c8b787 146 return;
bluefish 0:c86a79c8b787 147 }
bluefish 0:c86a79c8b787 148
bluefish 0:c86a79c8b787 149 void sensing_and_control(){
bluefish 0:c86a79c8b787 150 //LPF variables
bluefish 2:9c83de2d33ca 151 int16_t com = 0;
bluefish 2:9c83de2d33ca 152 int16_t comL = 0;
bluefish 2:9c83de2d33ca 153 int16_t comR = 0;
bluefish 0:c86a79c8b787 154 static float angle_int = 0.0f;
bluefish 0:c86a79c8b787 155 static float angle_adj = 0.0f;
bluefish 0:c86a79c8b787 156 static float angle_adj_t = 0.0f;
bluefish 0:c86a79c8b787 157
bluefish 0:c86a79c8b787 158 // read sensor
bluefish 0:c86a79c8b787 159 imu.read6Axis(
bluefish 0:c86a79c8b787 160 &sensor.acc[0], &sensor.acc[1], &sensor.acc[2],
bluefish 0:c86a79c8b787 161 &sensor.gyro[0], &sensor.gyro[1], &sensor.gyro[2]
bluefish 0:c86a79c8b787 162 );
bluefish 0:c86a79c8b787 163 #ifdef USE_SERIAL_DEBUG
bluefish 0:c86a79c8b787 164 usb_serial.printf( "%f\t %f\t %f\t %f\t %f\t %f\n",
bluefish 0:c86a79c8b787 165 sensor.acc[0], sensor.acc[1], sensor.acc[2],
bluefish 0:c86a79c8b787 166 sensor.gyro[0], sensor.gyro[1], sensor.gyro[2]
bluefish 0:c86a79c8b787 167 );
bluefish 0:c86a79c8b787 168 #endif
bluefish 0:c86a79c8b787 169
bluefish 0:c86a79c8b787 170 sensor.gyro[0] *= MPU9250G_DEG2RAD;
bluefish 0:c86a79c8b787 171 sensor.gyro[1] *= MPU9250G_DEG2RAD;
bluefish 0:c86a79c8b787 172 sensor.gyro[2] *= MPU9250G_DEG2RAD;
bluefish 0:c86a79c8b787 173
bluefish 0:c86a79c8b787 174 // filter
bluefish 0:c86a79c8b787 175 angle_adj_t = angle_adj;
bluefish 0:c86a79c8b787 176 angle_int += sensor.gyro[1] * PROCESS_INTERVAL_SEC; // integral gyro
bluefish 0:c86a79c8b787 177 angle_adj = -( sensor.acc[0] / sensor.acc[2] ) - angle_int; //
bluefish 0:c86a79c8b787 178 angle_adj = angle_adj_t + PROCESS_INTERVAL_SEC * ( angle_adj - angle_adj_t ) / 3.0f; // calc correction via LPF
bluefish 0:c86a79c8b787 179
bluefish 0:c86a79c8b787 180 // update state
bluefish 0:c86a79c8b787 181 posture.angle = angle_int + angle_adj;
bluefish 2:9c83de2d33ca 182 posture.angle_vel = sensor.gyro[1];
bluefish 2:9c83de2d33ca 183
bluefish 2:9c83de2d33ca 184 if( cnt > 0 ){ cnt--; }
bluefish 0:c86a79c8b787 185
bluefish 0:c86a79c8b787 186 #ifdef USE_SERIAL_DEBUG
bluefish 0:c86a79c8b787 187 usb_serial.printf( "%f\t %f\n",
bluefish 0:c86a79c8b787 188 posture.angle / MPU9250G_DEG2RAD,
bluefish 0:c86a79c8b787 189 posture.angle_vec / MPU9250G_DEG2RAD
bluefish 0:c86a79c8b787 190 );
bluefish 2:9c83de2d33ca 191 #endif
bluefish 0:c86a79c8b787 192
bluefish 0:c86a79c8b787 193 // control motor
bluefish 2:9c83de2d33ca 194 com = 0;
bluefish 2:9c83de2d33ca 195 com += control.K_angle * posture.angle;
bluefish 2:9c83de2d33ca 196 com += control.K_angle_vel * posture.angle_vel;
bluefish 2:9c83de2d33ca 197
bluefish 2:9c83de2d33ca 198 comL = com;
bluefish 2:9c83de2d33ca 199 comL += control.K_wheel * posture.wheel[0];
bluefish 2:9c83de2d33ca 200 comL += control.K_wheel_vel * posture.wheel[0];
bluefish 2:9c83de2d33ca 201 if( pad.enable && cnt > 0 ){
bluefish 2:9c83de2d33ca 202 comL += control.K_trans_vel * pad.y;
bluefish 2:9c83de2d33ca 203 comL += control.K_rot_vel * pad.x;
bluefish 2:9c83de2d33ca 204 }
bluefish 2:9c83de2d33ca 205
bluefish 2:9c83de2d33ca 206 comR = com;
bluefish 2:9c83de2d33ca 207 comR += control.K_wheel * posture.wheel[1];
bluefish 2:9c83de2d33ca 208 comR += control.K_wheel_vel * posture.wheel[1];
bluefish 2:9c83de2d33ca 209 if( pad.enable && cnt > 0 ){
bluefish 2:9c83de2d33ca 210 comR += control.K_trans_vel * pad.y;
bluefish 2:9c83de2d33ca 211 comR -= control.K_rot_vel * pad.x;
bluefish 2:9c83de2d33ca 212 }
bluefish 2:9c83de2d33ca 213
bluefish 2:9c83de2d33ca 214 if( posture.angle < MAX_ANGLE && posture.angle > -MAX_ANGLE ){
bluefish 2:9c83de2d33ca 215 controlMotor( comL, comR );
bluefish 2:9c83de2d33ca 216 }else{
bluefish 2:9c83de2d33ca 217 controlMotor( 0, 0 );
bluefish 2:9c83de2d33ca 218 }
bluefish 2:9c83de2d33ca 219
bluefish 2:9c83de2d33ca 220 receive_command();
bluefish 0:c86a79c8b787 221
bluefish 0:c86a79c8b787 222 return;
bluefish 0:c86a79c8b787 223 }
bluefish 2:9c83de2d33ca 224
bluefish 2:9c83de2d33ca 225 void receive_command(){
bluefish 2:9c83de2d33ca 226 uint8_t c = 0;
bluefish 2:9c83de2d33ca 227 if( bt.readable() ){
bluefish 2:9c83de2d33ca 228 c = bt.getc();
bluefish 2:9c83de2d33ca 229
bluefish 2:9c83de2d33ca 230 switch( c ){
bluefish 2:9c83de2d33ca 231 case 'L':
bluefish 2:9c83de2d33ca 232 pad.x = -1.0f;
bluefish 2:9c83de2d33ca 233 cnt = 100;
bluefish 2:9c83de2d33ca 234 break;
bluefish 2:9c83de2d33ca 235 case 'R':
bluefish 2:9c83de2d33ca 236 pad.x = 1.0f;
bluefish 2:9c83de2d33ca 237 cnt = 100;
bluefish 2:9c83de2d33ca 238 break;
bluefish 2:9c83de2d33ca 239 case 'S':
bluefish 2:9c83de2d33ca 240 pad.x = 0.0f;
bluefish 2:9c83de2d33ca 241 pad.y = 0.0f;
bluefish 2:9c83de2d33ca 242 cnt = 100;
bluefish 2:9c83de2d33ca 243 break;
bluefish 2:9c83de2d33ca 244 case 'I':
bluefish 2:9c83de2d33ca 245 pad.enable = 0x01;
bluefish 2:9c83de2d33ca 246 cnt = 100;
bluefish 2:9c83de2d33ca 247 break;
bluefish 2:9c83de2d33ca 248 case 'O':
bluefish 2:9c83de2d33ca 249 pad.enable = 0x00;
bluefish 2:9c83de2d33ca 250 cnt = 100;
bluefish 2:9c83de2d33ca 251 break;
bluefish 2:9c83de2d33ca 252 }
bluefish 2:9c83de2d33ca 253 }
bluefish 2:9c83de2d33ca 254 return;
bluefish 2:9c83de2d33ca 255 }