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?
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 | 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 | } |