Shimon AJISAKA / Mbed 2 deprecated InvertedPendulum2017

2-wheel Inverted Pendulum control program

Dependencies:   AsyncSerial Lib_DFPlayerMini Lib_MPU9250_SPI mbed

main.cpp@0:c86a79c8b787, 2017-09-15 (annotated)

Committer:
bluefish
Date:
Fri Sep 15 14:54:59 2017 +0000
Revision:
0:c86a79c8b787
Child:
1:a6cb5f642e69
???????

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