Shimon AJISAKA / Mbed 2 deprecated InvertedPendulum2017

2-wheel Inverted Pendulum control program

Dependencies:   AsyncSerial Lib_DFPlayerMini Lib_MPU9250_SPI mbed

main.cpp@6:a5f674c2f262, 2018-04-20 (annotated)

Committer:
bluefish
Date:
Fri Apr 20 18:14:15 2018 +0000
Revision:
6:a5f674c2f262
Parent:
5:f3cbcb294ba9
Child:
7:77174a098e6f
???

Who changed what in which revision?

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