Shimon AJISAKA / Mbed 2 deprecated InvertedPendulum2017

2-wheel Inverted Pendulum control program

Dependencies:   AsyncSerial Lib_DFPlayerMini Lib_MPU9250_SPI mbed

main.cpp@1:a6cb5f642e69, 2017-09-16 (annotated)

Committer:
bluefish
Date:
Sat Sep 16 15:09:53 2017 +0000
Revision:
1:a6cb5f642e69
Parent:
0:c86a79c8b787
Child:
2:9c83de2d33ca
??????

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 0:c86a79c8b787 13
bluefish 0:c86a79c8b787 14 // class instance
bluefish 0:c86a79c8b787 15 Ticker tic_sen_ctrl;
bluefish 0:c86a79c8b787 16 CAN can_driver( CAN_RX, CAN_TX );
bluefish 0:c86a79c8b787 17 MPU9250 imu( MPU9250_CS, MPU9250_MOSI, MPU9250_MISO, MPU9250_SCK );
bluefish 0:c86a79c8b787 18 DFPlayerMini player( DFPLAYER_BUSY, DFPLAYER_TX, DFPLAYER_RX );
bluefish 1:a6cb5f642e69 19 LocalFileSystem file_local( FILESYSTEM_PATH );
bluefish 0:c86a79c8b787 20
bluefish 0:c86a79c8b787 21 void beginMotor();
bluefish 0:c86a79c8b787 22 //void controlMotor( int16_t left, int16_t right );
bluefish 0:c86a79c8b787 23 void sensing_and_control();
bluefish 0:c86a79c8b787 24
bluefish 0:c86a79c8b787 25 int main() {
bluefish 0:c86a79c8b787 26 uint8_t isLoop = 0x01;
bluefish 1:a6cb5f642e69 27 FILE* fp;
bluefish 0:c86a79c8b787 28
bluefish 0:c86a79c8b787 29 #ifdef USE_SERIAL_DEBUG
bluefish 0:c86a79c8b787 30 usb_serial.baud( DEBUG_BAUDRATE );
bluefish 0:c86a79c8b787 31 usb_serial.format( 8, RawSerial::None, 1 );
bluefish 0:c86a79c8b787 32 usb_serial.printf( "USB Serial Debug Enable\n" );
bluefish 0:c86a79c8b787 33 #endif
bluefish 0:c86a79c8b787 34
bluefish 0:c86a79c8b787 35 // initialize control gain
bluefish 1:a6cb5f642e69 36 fp = fopen( FILESYSTEM_GAIN, "r" );
bluefish 1:a6cb5f642e69 37 fscanf( fp, "%f\r\n", &control.K_angle );
bluefish 1:a6cb5f642e69 38 fscanf( fp, "%f\r\n", &control.K_angle_vel );
bluefish 1:a6cb5f642e69 39 fscanf( fp, "%f\r\n", &control.K_wheel );
bluefish 1:a6cb5f642e69 40 fscanf( fp, "%f\r\n", &control.K_wheel_vel );
bluefish 1:a6cb5f642e69 41 fscanf( fp, "%f\r\n", &control.K_rot_vel );
bluefish 1:a6cb5f642e69 42 fscanf( fp, "%f\r\n", &control.K_trans_vel );
bluefish 1:a6cb5f642e69 43 fclose( fp );
bluefish 1:a6cb5f642e69 44
bluefish 1:a6cb5f642e69 45 control.K_angle /= MPU9250G_DEG2RAD;
bluefish 1:a6cb5f642e69 46 control.K_angle_vel /= MPU9250G_DEG2RAD;
bluefish 0:c86a79c8b787 47
bluefish 0:c86a79c8b787 48 // initialize CAN
bluefish 0:c86a79c8b787 49 can_driver.frequency( 500000 );
bluefish 0:c86a79c8b787 50
bluefish 0:c86a79c8b787 51 // initialize sensor
bluefish 0:c86a79c8b787 52 imu.begin();
bluefish 0:c86a79c8b787 53 imu.setAccelRange( BITS_FS_16G );
bluefish 0:c86a79c8b787 54 imu.setGyroRange( BITS_FS_2000DPS );
bluefish 0:c86a79c8b787 55
bluefish 0:c86a79c8b787 56 // initialize player
bluefish 0:c86a79c8b787 57 /*
bluefish 0:c86a79c8b787 58 player.begin();
bluefish 0:c86a79c8b787 59 player.volumeSet( 5 );
bluefish 0:c86a79c8b787 60 player.playNumber( 1 );
bluefish 0:c86a79c8b787 61 */
bluefish 0:c86a79c8b787 62
bluefish 0:c86a79c8b787 63 // wait motor driver ON
bluefish 0:c86a79c8b787 64 wait( 0.1f );
bluefish 0:c86a79c8b787 65
bluefish 0:c86a79c8b787 66 // move driver to operation
bluefish 0:c86a79c8b787 67 beginMotor();
bluefish 0:c86a79c8b787 68
bluefish 0:c86a79c8b787 69 // start ticker interrupt
bluefish 0:c86a79c8b787 70 tic_sen_ctrl.attach( sensing_and_control, PROCESS_INTERVAL_SEC );
bluefish 0:c86a79c8b787 71
bluefish 0:c86a79c8b787 72 // main loop
bluefish 0:c86a79c8b787 73 while( isLoop ){
bluefish 0:c86a79c8b787 74 }
bluefish 0:c86a79c8b787 75 return 0;
bluefish 0:c86a79c8b787 76 }
bluefish 0:c86a79c8b787 77
bluefish 0:c86a79c8b787 78 void beginMotor(){
bluefish 0:c86a79c8b787 79 CANMessage msg;
bluefish 0:c86a79c8b787 80
bluefish 0:c86a79c8b787 81 msg.format = CANStandard;
bluefish 0:c86a79c8b787 82 msg.type = CANData;
bluefish 0:c86a79c8b787 83 msg.id = 0x410;
bluefish 0:c86a79c8b787 84 msg.len = 2;
bluefish 0:c86a79c8b787 85 msg.data[0] = DEVID_LEFT;
bluefish 0:c86a79c8b787 86 msg.data[1] = 0x06;
bluefish 0:c86a79c8b787 87 can_driver.write( msg );
bluefish 0:c86a79c8b787 88
bluefish 0:c86a79c8b787 89 wait( 0.005f );
bluefish 0:c86a79c8b787 90
bluefish 0:c86a79c8b787 91 msg.format = CANStandard;
bluefish 0:c86a79c8b787 92 msg.type = CANData;
bluefish 0:c86a79c8b787 93 msg.id = 0x410;
bluefish 0:c86a79c8b787 94 msg.len = 2;
bluefish 0:c86a79c8b787 95 msg.data[0] = DEVID_RIGHT;
bluefish 0:c86a79c8b787 96 msg.data[1] = 0x06;
bluefish 0:c86a79c8b787 97 can_driver.write( msg );
bluefish 0:c86a79c8b787 98
bluefish 0:c86a79c8b787 99 return;
bluefish 0:c86a79c8b787 100 }
bluefish 0:c86a79c8b787 101
bluefish 0:c86a79c8b787 102 void controlMotor( int16_t left, int16_t right ){
bluefish 0:c86a79c8b787 103 CANMessage msg;
bluefish 0:c86a79c8b787 104
bluefish 0:c86a79c8b787 105 left = -left;
bluefish 0:c86a79c8b787 106 msg.format = CANStandard;
bluefish 0:c86a79c8b787 107 msg.type = CANData;
bluefish 0:c86a79c8b787 108 msg.id = 0x420;
bluefish 0:c86a79c8b787 109 msg.len = 8;
bluefish 0:c86a79c8b787 110 msg.data[0] = DEVID_LEFT;
bluefish 0:c86a79c8b787 111 msg.data[1] = 0x06;
bluefish 0:c86a79c8b787 112 msg.data[2] = 0x00;
bluefish 0:c86a79c8b787 113 msg.data[3] = 0x00;
bluefish 0:c86a79c8b787 114 msg.data[4] = ( left >> 8 ) & 0xFF;
bluefish 0:c86a79c8b787 115 msg.data[5] = ( left >> 0 ) & 0xFF;
bluefish 0:c86a79c8b787 116 msg.data[6] = 0x00;
bluefish 0:c86a79c8b787 117 msg.data[7] = 0x00;
bluefish 0:c86a79c8b787 118 can_driver.write( msg );
bluefish 0:c86a79c8b787 119
bluefish 0:c86a79c8b787 120 wait( 0.005f );
bluefish 0:c86a79c8b787 121
bluefish 0:c86a79c8b787 122 right = right;
bluefish 0:c86a79c8b787 123 msg.format = CANStandard;
bluefish 0:c86a79c8b787 124 msg.type = CANData;
bluefish 0:c86a79c8b787 125 msg.id = 0x420;
bluefish 0:c86a79c8b787 126 msg.len = 8;
bluefish 0:c86a79c8b787 127 msg.data[0] = DEVID_RIGHT;
bluefish 0:c86a79c8b787 128 msg.data[1] = 0x06;
bluefish 0:c86a79c8b787 129 msg.data[2] = 0x00;
bluefish 0:c86a79c8b787 130 msg.data[3] = 0x00;
bluefish 0:c86a79c8b787 131 msg.data[4] = ( right >> 8 ) & 0xFF;
bluefish 0:c86a79c8b787 132 msg.data[5] = ( right >> 0 ) & 0xFF;
bluefish 0:c86a79c8b787 133 msg.data[6] = 0x00;
bluefish 0:c86a79c8b787 134 msg.data[7] = 0x00;
bluefish 0:c86a79c8b787 135 can_driver.write( msg );
bluefish 0:c86a79c8b787 136
bluefish 0:c86a79c8b787 137 return;
bluefish 0:c86a79c8b787 138 }
bluefish 0:c86a79c8b787 139
bluefish 0:c86a79c8b787 140 void sensing_and_control(){
bluefish 0:c86a79c8b787 141 //LPF variables
bluefish 0:c86a79c8b787 142 int16_t com = 0;
bluefish 0:c86a79c8b787 143 static float angle_int = 0.0f;
bluefish 0:c86a79c8b787 144 static float angle_adj = 0.0f;
bluefish 0:c86a79c8b787 145 static float angle_adj_t = 0.0f;
bluefish 0:c86a79c8b787 146
bluefish 0:c86a79c8b787 147 // read sensor
bluefish 0:c86a79c8b787 148 imu.read6Axis(
bluefish 0:c86a79c8b787 149 &sensor.acc[0], &sensor.acc[1], &sensor.acc[2],
bluefish 0:c86a79c8b787 150 &sensor.gyro[0], &sensor.gyro[1], &sensor.gyro[2]
bluefish 0:c86a79c8b787 151 );
bluefish 0:c86a79c8b787 152 #ifdef USE_SERIAL_DEBUG
bluefish 0:c86a79c8b787 153 usb_serial.printf( "%f\t %f\t %f\t %f\t %f\t %f\n",
bluefish 0:c86a79c8b787 154 sensor.acc[0], sensor.acc[1], sensor.acc[2],
bluefish 0:c86a79c8b787 155 sensor.gyro[0], sensor.gyro[1], sensor.gyro[2]
bluefish 0:c86a79c8b787 156 );
bluefish 0:c86a79c8b787 157 #endif
bluefish 0:c86a79c8b787 158
bluefish 0:c86a79c8b787 159 sensor.gyro[0] *= MPU9250G_DEG2RAD;
bluefish 0:c86a79c8b787 160 sensor.gyro[1] *= MPU9250G_DEG2RAD;
bluefish 0:c86a79c8b787 161 sensor.gyro[2] *= MPU9250G_DEG2RAD;
bluefish 0:c86a79c8b787 162
bluefish 0:c86a79c8b787 163 // filter
bluefish 0:c86a79c8b787 164 angle_adj_t = angle_adj;
bluefish 0:c86a79c8b787 165 angle_int += sensor.gyro[1] * PROCESS_INTERVAL_SEC; // integral gyro
bluefish 0:c86a79c8b787 166 angle_adj = -( sensor.acc[0] / sensor.acc[2] ) - angle_int; //
bluefish 0:c86a79c8b787 167 angle_adj = angle_adj_t + PROCESS_INTERVAL_SEC * ( angle_adj - angle_adj_t ) / 3.0f; // calc correction via LPF
bluefish 0:c86a79c8b787 168
bluefish 0:c86a79c8b787 169 // update state
bluefish 0:c86a79c8b787 170 posture.angle = angle_int + angle_adj;
bluefish 0:c86a79c8b787 171 posture.angle_vec = sensor.gyro[1];
bluefish 0:c86a79c8b787 172
bluefish 0:c86a79c8b787 173 #ifdef USE_SERIAL_DEBUG
bluefish 0:c86a79c8b787 174 usb_serial.printf( "%f\t %f\n",
bluefish 0:c86a79c8b787 175 posture.angle / MPU9250G_DEG2RAD,
bluefish 0:c86a79c8b787 176 posture.angle_vec / MPU9250G_DEG2RAD
bluefish 0:c86a79c8b787 177 );
bluefish 0:c86a79c8b787 178 #endif
bluefish 0:c86a79c8b787 179
bluefish 0:c86a79c8b787 180 // control motor
bluefish 0:c86a79c8b787 181 com = (int16_t)(
bluefish 0:c86a79c8b787 182 posture.angle * control.K_angle
bluefish 0:c86a79c8b787 183 + posture.angle_vec * control.K_angle_vel );
bluefish 0:c86a79c8b787 184 controlMotor( com, com );
bluefish 0:c86a79c8b787 185
bluefish 0:c86a79c8b787 186 return;
bluefish 0:c86a79c8b787 187 }