2-wheel Inverted Pendulum control program
Dependencies: AsyncSerial Lib_DFPlayerMini Lib_MPU9250_SPI mbed
Diff: main.cpp
- Revision:
- 0:c86a79c8b787
- Child:
- 1:a6cb5f642e69
diff -r 000000000000 -r c86a79c8b787 main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Fri Sep 15 14:54:59 2017 +0000 @@ -0,0 +1,175 @@ +#include "mbed.h" +#include "defines.h" + +#define K_ANGLE (1000.0f) +#define K_ANGLE_VEL (5.0f) + +STRUCTSENSOR sensor; +STRUCTPOSTURE posture; +STRUCTCONRTOLPARAM control; + + + +// class instance +Ticker tic_sen_ctrl; +CAN can_driver( CAN_RX, CAN_TX ); +MPU9250 imu( MPU9250_CS, MPU9250_MOSI, MPU9250_MISO, MPU9250_SCK ); +DFPlayerMini player( DFPLAYER_BUSY, DFPLAYER_TX, DFPLAYER_RX ); + +void beginMotor(); +//void controlMotor( int16_t left, int16_t right ); +void sensing_and_control(); + +int main() { + uint8_t isLoop = 0x01; + + #ifdef USE_SERIAL_DEBUG + usb_serial.baud( DEBUG_BAUDRATE ); + usb_serial.format( 8, RawSerial::None, 1 ); + usb_serial.printf( "USB Serial Debug Enable\n" ); + #endif + + // initialize control gain + control.K_angle = K_ANGLE / MPU9250G_DEG2RAD; + control.K_angle_vel = K_ANGLE_VEL / MPU9250G_DEG2RAD; + + // initialize CAN + can_driver.frequency( 500000 ); + + // initialize sensor + imu.begin(); + imu.setAccelRange( BITS_FS_16G ); + imu.setGyroRange( BITS_FS_2000DPS ); + + // initialize player + /* + player.begin(); + player.volumeSet( 5 ); + player.playNumber( 1 ); + */ + + // wait motor driver ON + wait( 0.1f ); + + // move driver to operation + beginMotor(); + + // start ticker interrupt + tic_sen_ctrl.attach( sensing_and_control, PROCESS_INTERVAL_SEC ); + + // main loop + while( isLoop ){ + } + return 0; +} + +void beginMotor(){ + CANMessage msg; + + msg.format = CANStandard; + msg.type = CANData; + msg.id = 0x410; + msg.len = 2; + msg.data[0] = DEVID_LEFT; + msg.data[1] = 0x06; + can_driver.write( msg ); + + wait( 0.005f ); + + msg.format = CANStandard; + msg.type = CANData; + msg.id = 0x410; + msg.len = 2; + msg.data[0] = DEVID_RIGHT; + msg.data[1] = 0x06; + can_driver.write( msg ); + + return; +} + +void controlMotor( int16_t left, int16_t right ){ + CANMessage msg; + + left = -left; + msg.format = CANStandard; + msg.type = CANData; + msg.id = 0x420; + msg.len = 8; + msg.data[0] = DEVID_LEFT; + msg.data[1] = 0x06; + msg.data[2] = 0x00; + msg.data[3] = 0x00; + msg.data[4] = ( left >> 8 ) & 0xFF; + msg.data[5] = ( left >> 0 ) & 0xFF; + msg.data[6] = 0x00; + msg.data[7] = 0x00; + can_driver.write( msg ); + + wait( 0.005f ); + + right = right; + msg.format = CANStandard; + msg.type = CANData; + msg.id = 0x420; + msg.len = 8; + msg.data[0] = DEVID_RIGHT; + msg.data[1] = 0x06; + msg.data[2] = 0x00; + msg.data[3] = 0x00; + msg.data[4] = ( right >> 8 ) & 0xFF; + msg.data[5] = ( right >> 0 ) & 0xFF; + msg.data[6] = 0x00; + msg.data[7] = 0x00; + can_driver.write( msg ); + + return; +} + +void sensing_and_control(){ + //LPF variables + int16_t com = 0; + static float angle_int = 0.0f; + static float angle_adj = 0.0f; + static float angle_adj_t = 0.0f; + + // read sensor + imu.read6Axis( + &sensor.acc[0], &sensor.acc[1], &sensor.acc[2], + &sensor.gyro[0], &sensor.gyro[1], &sensor.gyro[2] + ); + #ifdef USE_SERIAL_DEBUG + usb_serial.printf( "%f\t %f\t %f\t %f\t %f\t %f\n", + sensor.acc[0], sensor.acc[1], sensor.acc[2], + sensor.gyro[0], sensor.gyro[1], sensor.gyro[2] + ); + #endif + + sensor.gyro[0] *= MPU9250G_DEG2RAD; + sensor.gyro[1] *= MPU9250G_DEG2RAD; + sensor.gyro[2] *= MPU9250G_DEG2RAD; + + // filter + angle_adj_t = angle_adj; + angle_int += sensor.gyro[1] * PROCESS_INTERVAL_SEC; // integral gyro + angle_adj = -( sensor.acc[0] / sensor.acc[2] ) - angle_int; // + angle_adj = angle_adj_t + PROCESS_INTERVAL_SEC * ( angle_adj - angle_adj_t ) / 3.0f; // calc correction via LPF + + // update state + posture.angle = angle_int + angle_adj; + posture.angle_vec = sensor.gyro[1]; + + #ifdef USE_SERIAL_DEBUG + usb_serial.printf( "%f\t %f\n", + posture.angle / MPU9250G_DEG2RAD, + posture.angle_vec / MPU9250G_DEG2RAD + ); + #endif + + // control motor + com = (int16_t)( + posture.angle * control.K_angle + + posture.angle_vec * control.K_angle_vel ); + controlMotor( com, com ); + + return; +}