2-wheel Inverted Pendulum control program
Dependencies: AsyncSerial Lib_DFPlayerMini Lib_MPU9250_SPI mbed
main.cpp@7:77174a098e6f, 2018-05-02 (annotated)
- Committer:
- bluefish
- Date:
- Wed May 02 10:57:10 2018 +0000
- Revision:
- 7:77174a098e6f
- Parent:
- 6:a5f674c2f262
for peshala project
Who changed what in which revision?
User | Revision | Line number | New 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 | 0:c86a79c8b787 | 13 | STRUCTPOSTURE posture; |
bluefish | 0:c86a79c8b787 | 14 | STRUCTCONRTOLPARAM control; |
bluefish | 7:77174a098e6f | 15 | STRUCTGAMEPAD pad; |
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 | 7:77174a098e6f | 26 | |
bluefish | 6:a5f674c2f262 | 27 | #ifdef USE_FILESYSTEM |
bluefish | 6:a5f674c2f262 | 28 | LocalFileSystem file_local( FILESYSTEM_PATH ); |
bluefish | 6:a5f674c2f262 | 29 | #endif |
bluefish | 0:c86a79c8b787 | 30 | |
bluefish | 7:77174a098e6f | 31 | // variables |
bluefish | 7:77174a098e6f | 32 | float C_dead_angle = 0.0f; |
bluefish | 7:77174a098e6f | 33 | float C_max_angle = 0.0f; |
bluefish | 7:77174a098e6f | 34 | float C_offset_angle = 0.0f; |
bluefish | 7:77174a098e6f | 35 | int16_t C_VD = 0; |
bluefish | 7:77174a098e6f | 36 | |
bluefish | 7:77174a098e6f | 37 | // function prototype |
bluefish | 0:c86a79c8b787 | 38 | void beginMotor(); |
bluefish | 4:72c26c07c251 | 39 | void controlMotor( int16_t left, int16_t right ); |
bluefish | 0:c86a79c8b787 | 40 | void sensing_and_control(); |
bluefish | 4:72c26c07c251 | 41 | //void receive_command(); |
bluefish | 7:77174a098e6f | 42 | float convPadJoystick( float input, const float val_deadzone, const float val_max ); |
bluefish | 2:9c83de2d33ca | 43 | |
bluefish | 0:c86a79c8b787 | 44 | int main() { |
bluefish | 0:c86a79c8b787 | 45 | uint8_t isLoop = 0x01; |
bluefish | 0:c86a79c8b787 | 46 | |
bluefish | 0:c86a79c8b787 | 47 | #ifdef USE_SERIAL_DEBUG |
bluefish | 0:c86a79c8b787 | 48 | usb_serial.baud( DEBUG_BAUDRATE ); |
bluefish | 0:c86a79c8b787 | 49 | usb_serial.format( 8, RawSerial::None, 1 ); |
bluefish | 0:c86a79c8b787 | 50 | usb_serial.printf( "USB Serial Debug Enable\n" ); |
bluefish | 0:c86a79c8b787 | 51 | #endif |
bluefish | 0:c86a79c8b787 | 52 | |
bluefish | 7:77174a098e6f | 53 | #ifdef USE_LED_DEBUG |
bluefish | 7:77174a098e6f | 54 | myled[0] = 0; |
bluefish | 7:77174a098e6f | 55 | myled[1] = 0; |
bluefish | 7:77174a098e6f | 56 | myled[2] = 0; |
bluefish | 7:77174a098e6f | 57 | myled[3] = 0; |
bluefish | 7:77174a098e6f | 58 | #endif |
bluefish | 7:77174a098e6f | 59 | |
bluefish | 0:c86a79c8b787 | 60 | // initialize control gain |
bluefish | 6:a5f674c2f262 | 61 | #ifdef USE_FILESYSTEM |
bluefish | 6:a5f674c2f262 | 62 | FILE* fp; |
bluefish | 6:a5f674c2f262 | 63 | fp = fopen( FILESYSTEM_GAIN, "r" ); |
bluefish | 7:77174a098e6f | 64 | // motor control parameter |
bluefish | 7:77174a098e6f | 65 | fscanf( fp, "%d\r\n", &C_VD ); |
bluefish | 7:77174a098e6f | 66 | // control gain |
bluefish | 6:a5f674c2f262 | 67 | fscanf( fp, "%f\r\n", &control.K_angle ); |
bluefish | 6:a5f674c2f262 | 68 | fscanf( fp, "%f\r\n", &control.K_angle_vel ); |
bluefish | 6:a5f674c2f262 | 69 | fscanf( fp, "%f\r\n", &control.K_wheel ); |
bluefish | 6:a5f674c2f262 | 70 | fscanf( fp, "%f\r\n", &control.K_wheel_vel ); |
bluefish | 6:a5f674c2f262 | 71 | fscanf( fp, "%f\r\n", &control.K_rot_vel ); |
bluefish | 6:a5f674c2f262 | 72 | fscanf( fp, "%f\r\n", &control.K_trans_vel ); |
bluefish | 7:77174a098e6f | 73 | // control parameter |
bluefish | 6:a5f674c2f262 | 74 | fscanf( fp, "%f\r\n", &control.C_max_angle ); |
bluefish | 7:77174a098e6f | 75 | fscanf( fp, "%f\r\n", &control.C_offset_angle ); |
bluefish | 7:77174a098e6f | 76 | // pad parameter |
bluefish | 7:77174a098e6f | 77 | fscanf( fp, "%f\r\n", &C_dead_angle ); |
bluefish | 7:77174a098e6f | 78 | fscanf( fp, "%f\r\n", &C_max_angle ); |
bluefish | 7:77174a098e6f | 79 | fscanf( fp, "%f\r\n", &C_offset_angle ); |
bluefish | 6:a5f674c2f262 | 80 | fclose( fp ); |
bluefish | 6:a5f674c2f262 | 81 | #else |
bluefish | 7:77174a098e6f | 82 | // control gain |
bluefish | 7:77174a098e6f | 83 | C_VD = 0; |
bluefish | 7:77174a098e6f | 84 | control.K_angle = 800.0f; |
bluefish | 7:77174a098e6f | 85 | control.K_angle_vel = 5.0f; |
bluefish | 7:77174a098e6f | 86 | control.K_wheel = 0.0f; |
bluefish | 7:77174a098e6f | 87 | control.K_wheel_vel = 0.0f; |
bluefish | 7:77174a098e6f | 88 | control.K_rot_vel = 0.0f; |
bluefish | 7:77174a098e6f | 89 | control.K_trans_vel = 0.0f; |
bluefish | 7:77174a098e6f | 90 | // control parameter |
bluefish | 7:77174a098e6f | 91 | control.C_max_angle = 15.0f; |
bluefish | 7:77174a098e6f | 92 | control.C_offset_angle = 0.0f; |
bluefish | 7:77174a098e6f | 93 | // pad parameter |
bluefish | 7:77174a098e6f | 94 | C_dead_angle = 5.0f; |
bluefish | 7:77174a098e6f | 95 | C_max_angle = 11.0f; |
bluefish | 7:77174a098e6f | 96 | C_offset_angle = 0.0f; |
bluefish | 7:77174a098e6f | 97 | /* |
bluefish | 7:77174a098e6f | 98 | // control gain |
bluefish | 7:77174a098e6f | 99 | control.K_angle = 800.0f; |
bluefish | 7:77174a098e6f | 100 | control.K_angle_vel = 5.0f; |
bluefish | 7:77174a098e6f | 101 | control.K_wheel = 0.0f; |
bluefish | 7:77174a098e6f | 102 | control.K_wheel_vel = 0.0f; |
bluefish | 7:77174a098e6f | 103 | control.K_rot_vel = 0.0f; |
bluefish | 7:77174a098e6f | 104 | control.K_trans_vel = 0.0f; |
bluefish | 7:77174a098e6f | 105 | // control parameter |
bluefish | 7:77174a098e6f | 106 | control.C_max_angle = 15.0f; |
bluefish | 7:77174a098e6f | 107 | control.C_offset_angle = 0.0f; |
bluefish | 7:77174a098e6f | 108 | // pad parameter |
bluefish | 7:77174a098e6f | 109 | C_dead_angle = 5.0f; |
bluefish | 7:77174a098e6f | 110 | C_max_angle = 11.0f; |
bluefish | 7:77174a098e6f | 111 | C_offset_angle = 0.0f; |
bluefish | 7:77174a098e6f | 112 | */ |
bluefish | 6:a5f674c2f262 | 113 | #endif |
bluefish | 6:a5f674c2f262 | 114 | |
bluefish | 7:77174a098e6f | 115 | #ifdef USE_SERIAL_DEBUG |
bluefish | 7:77174a098e6f | 116 | usb_serial.printf( "MOTOR PARAMETER\n" ); |
bluefish | 7:77174a098e6f | 117 | usb_serial.printf( "C_VD :\t%d\n", C_VD ); |
bluefish | 7:77174a098e6f | 118 | usb_serial.printf( "CONTROL GAIN\n" ); |
bluefish | 7:77174a098e6f | 119 | usb_serial.printf( "K_angle :\t%f\n", control.K_angle ); |
bluefish | 7:77174a098e6f | 120 | usb_serial.printf( "K_angle_vel :\t%f\n", control.K_angle_vel ); |
bluefish | 7:77174a098e6f | 121 | usb_serial.printf( "K_wheel :\t%f\n", control.K_wheel ); |
bluefish | 7:77174a098e6f | 122 | usb_serial.printf( "K_wheel_vel :\t%f\n", control.K_wheel_vel ); |
bluefish | 7:77174a098e6f | 123 | usb_serial.printf( "K_rot_vel :\t%f\n", control.K_rot_vel ); |
bluefish | 7:77174a098e6f | 124 | usb_serial.printf( "K_trans_vel :\t%f\n", control.K_trans_vel ); |
bluefish | 7:77174a098e6f | 125 | usb_serial.printf( "CONTROL PARAMETER\n" ); |
bluefish | 7:77174a098e6f | 126 | usb_serial.printf( "C_max_angle :\t%f\n", control.C_max_angle ); |
bluefish | 7:77174a098e6f | 127 | usb_serial.printf( "C_offset_angle :\t%f\n", control.C_offset_angle ); |
bluefish | 7:77174a098e6f | 128 | usb_serial.printf( "PAD PARAMETER\n" ); |
bluefish | 7:77174a098e6f | 129 | usb_serial.printf( "C_dead_angle :\t%f\n", C_dead_angle ); |
bluefish | 7:77174a098e6f | 130 | usb_serial.printf( "C_max_angle :\t%f\n", C_max_angle ); |
bluefish | 7:77174a098e6f | 131 | usb_serial.printf( "C_offset_angle :\t%f\n", C_offset_angle ); |
bluefish | 7:77174a098e6f | 132 | wait( 5.0f ); |
bluefish | 7:77174a098e6f | 133 | #endif |
bluefish | 1:a6cb5f642e69 | 134 | |
bluefish | 4:72c26c07c251 | 135 | // coefficient |
bluefish | 1:a6cb5f642e69 | 136 | control.K_angle /= MPU9250G_DEG2RAD; |
bluefish | 1:a6cb5f642e69 | 137 | control.K_angle_vel /= MPU9250G_DEG2RAD; |
bluefish | 4:72c26c07c251 | 138 | control.C_max_angle /= MPU9250G_DEG2RAD; |
bluefish | 0:c86a79c8b787 | 139 | |
bluefish | 0:c86a79c8b787 | 140 | // initialize CAN |
bluefish | 0:c86a79c8b787 | 141 | can_driver.frequency( 500000 ); |
bluefish | 0:c86a79c8b787 | 142 | |
bluefish | 0:c86a79c8b787 | 143 | // initialize sensor |
bluefish | 7:77174a098e6f | 144 | #ifdef USE_FIRST_IMU |
bluefish | 7:77174a098e6f | 145 | imu_vehicle.begin(); |
bluefish | 7:77174a098e6f | 146 | imu_vehicle.setAccelRange( BITS_FS_16G ); |
bluefish | 7:77174a098e6f | 147 | imu_vehicle.setGyroRange( BITS_FS_2000DPS ); |
bluefish | 7:77174a098e6f | 148 | init_comp_filter( &cf_vehicle, 3.0f ); |
bluefish | 7:77174a098e6f | 149 | #endif |
bluefish | 7:77174a098e6f | 150 | |
bluefish | 7:77174a098e6f | 151 | #ifdef USE_SECOND_IMU |
bluefish | 7:77174a098e6f | 152 | imu_interface.begin(); |
bluefish | 7:77174a098e6f | 153 | imu_interface.setAccelRange( BITS_FS_16G ); |
bluefish | 7:77174a098e6f | 154 | imu_interface.setGyroRange( BITS_FS_2000DPS ); |
bluefish | 7:77174a098e6f | 155 | init_comp_filter( &cf_interface, 3.0f ); |
bluefish | 7:77174a098e6f | 156 | #endif |
bluefish | 0:c86a79c8b787 | 157 | |
bluefish | 7:77174a098e6f | 158 | #ifdef USE_SERIAL_DEBUG |
bluefish | 7:77174a098e6f | 159 | #ifdef USE_FIRST_IMU |
bluefish | 7:77174a098e6f | 160 | usb_serial.printf( "IMU1 Who am I..." ); |
bluefish | 7:77174a098e6f | 161 | while( imu_vehicle.getWhoAmI() != 0x73 && imu_vehicle.getWhoAmI() != 0x71 ){ wait(0.1); } |
bluefish | 7:77174a098e6f | 162 | usb_serial.printf( "ok\n" ); |
bluefish | 7:77174a098e6f | 163 | #endif |
bluefish | 7:77174a098e6f | 164 | #endif |
bluefish | 7:77174a098e6f | 165 | #ifdef USE_LED_DEBUG |
bluefish | 7:77174a098e6f | 166 | myled[0] = 1; |
bluefish | 7:77174a098e6f | 167 | #endif |
bluefish | 7:77174a098e6f | 168 | #ifdef USE_SERIAL_DEBUG |
bluefish | 7:77174a098e6f | 169 | #ifdef USE_SECOND_IMU |
bluefish | 7:77174a098e6f | 170 | usb_serial.printf( "IMU2 Who am I..." ); |
bluefish | 7:77174a098e6f | 171 | while( imu_interface.getWhoAmI() != 0x73 && imu_interface.getWhoAmI() != 0x71 ){ wait(0.1); } |
bluefish | 7:77174a098e6f | 172 | usb_serial.printf( "ok\n" ); |
bluefish | 7:77174a098e6f | 173 | #endif |
bluefish | 7:77174a098e6f | 174 | #endif |
bluefish | 7:77174a098e6f | 175 | #ifdef USE_LED_DEBUG |
bluefish | 7:77174a098e6f | 176 | myled[1] = 1; |
bluefish | 7:77174a098e6f | 177 | #endif |
bluefish | 7:77174a098e6f | 178 | |
bluefish | 2:9c83de2d33ca | 179 | |
bluefish | 6:a5f674c2f262 | 180 | // initialize MP3 player |
bluefish | 0:c86a79c8b787 | 181 | /* |
bluefish | 0:c86a79c8b787 | 182 | player.begin(); |
bluefish | 0:c86a79c8b787 | 183 | player.volumeSet( 5 ); |
bluefish | 0:c86a79c8b787 | 184 | player.playNumber( 1 ); |
bluefish | 0:c86a79c8b787 | 185 | */ |
bluefish | 0:c86a79c8b787 | 186 | |
bluefish | 0:c86a79c8b787 | 187 | // wait motor driver ON |
bluefish | 7:77174a098e6f | 188 | wait( 0.1f ); |
bluefish | 0:c86a79c8b787 | 189 | |
bluefish | 0:c86a79c8b787 | 190 | // move driver to operation |
bluefish | 0:c86a79c8b787 | 191 | beginMotor(); |
bluefish | 0:c86a79c8b787 | 192 | |
bluefish | 0:c86a79c8b787 | 193 | // start ticker interrupt |
bluefish | 0:c86a79c8b787 | 194 | tic_sen_ctrl.attach( sensing_and_control, PROCESS_INTERVAL_SEC ); |
bluefish | 0:c86a79c8b787 | 195 | |
bluefish | 0:c86a79c8b787 | 196 | // main loop |
bluefish | 0:c86a79c8b787 | 197 | while( isLoop ){ |
bluefish | 0:c86a79c8b787 | 198 | } |
bluefish | 0:c86a79c8b787 | 199 | return 0; |
bluefish | 0:c86a79c8b787 | 200 | } |
bluefish | 0:c86a79c8b787 | 201 | |
bluefish | 0:c86a79c8b787 | 202 | void beginMotor(){ |
bluefish | 0:c86a79c8b787 | 203 | CANMessage msg; |
bluefish | 0:c86a79c8b787 | 204 | |
bluefish | 0:c86a79c8b787 | 205 | msg.format = CANStandard; |
bluefish | 0:c86a79c8b787 | 206 | msg.type = CANData; |
bluefish | 0:c86a79c8b787 | 207 | msg.id = 0x410; |
bluefish | 0:c86a79c8b787 | 208 | msg.len = 2; |
bluefish | 0:c86a79c8b787 | 209 | msg.data[0] = DEVID_LEFT; |
bluefish | 0:c86a79c8b787 | 210 | msg.data[1] = 0x06; |
bluefish | 0:c86a79c8b787 | 211 | can_driver.write( msg ); |
bluefish | 0:c86a79c8b787 | 212 | |
bluefish | 0:c86a79c8b787 | 213 | msg.format = CANStandard; |
bluefish | 0:c86a79c8b787 | 214 | msg.type = CANData; |
bluefish | 0:c86a79c8b787 | 215 | msg.id = 0x410; |
bluefish | 0:c86a79c8b787 | 216 | msg.len = 2; |
bluefish | 0:c86a79c8b787 | 217 | msg.data[0] = DEVID_RIGHT; |
bluefish | 0:c86a79c8b787 | 218 | msg.data[1] = 0x06; |
bluefish | 0:c86a79c8b787 | 219 | can_driver.write( msg ); |
bluefish | 0:c86a79c8b787 | 220 | |
bluefish | 0:c86a79c8b787 | 221 | return; |
bluefish | 0:c86a79c8b787 | 222 | } |
bluefish | 0:c86a79c8b787 | 223 | |
bluefish | 0:c86a79c8b787 | 224 | void controlMotor( int16_t left, int16_t right ){ |
bluefish | 0:c86a79c8b787 | 225 | CANMessage msg; |
bluefish | 7:77174a098e6f | 226 | int16_t cvd = 0; |
bluefish | 7:77174a098e6f | 227 | |
bluefish | 0:c86a79c8b787 | 228 | left = -left; |
bluefish | 7:77174a098e6f | 229 | cvd = (left < 0) ? -C_VD : C_VD; |
bluefish | 0:c86a79c8b787 | 230 | msg.format = CANStandard; |
bluefish | 0:c86a79c8b787 | 231 | msg.type = CANData; |
bluefish | 0:c86a79c8b787 | 232 | msg.id = 0x420; |
bluefish | 0:c86a79c8b787 | 233 | msg.len = 8; |
bluefish | 0:c86a79c8b787 | 234 | msg.data[0] = DEVID_LEFT; |
bluefish | 0:c86a79c8b787 | 235 | msg.data[1] = 0x06; |
bluefish | 0:c86a79c8b787 | 236 | msg.data[2] = 0x00; |
bluefish | 0:c86a79c8b787 | 237 | msg.data[3] = 0x00; |
bluefish | 0:c86a79c8b787 | 238 | msg.data[4] = ( left >> 8 ) & 0xFF; |
bluefish | 0:c86a79c8b787 | 239 | msg.data[5] = ( left >> 0 ) & 0xFF; |
bluefish | 7:77174a098e6f | 240 | msg.data[6] = ( cvd >> 8 ) & 0xFF; |
bluefish | 7:77174a098e6f | 241 | msg.data[7] = ( cvd >> 0 ) & 0xFF; |
bluefish | 0:c86a79c8b787 | 242 | can_driver.write( msg ); |
bluefish | 0:c86a79c8b787 | 243 | |
bluefish | 0:c86a79c8b787 | 244 | right = right; |
bluefish | 7:77174a098e6f | 245 | cvd = (right < 0) ? -C_VD : C_VD; |
bluefish | 0:c86a79c8b787 | 246 | msg.format = CANStandard; |
bluefish | 0:c86a79c8b787 | 247 | msg.type = CANData; |
bluefish | 0:c86a79c8b787 | 248 | msg.id = 0x420; |
bluefish | 0:c86a79c8b787 | 249 | msg.len = 8; |
bluefish | 0:c86a79c8b787 | 250 | msg.data[0] = DEVID_RIGHT; |
bluefish | 0:c86a79c8b787 | 251 | msg.data[1] = 0x06; |
bluefish | 0:c86a79c8b787 | 252 | msg.data[2] = 0x00; |
bluefish | 0:c86a79c8b787 | 253 | msg.data[3] = 0x00; |
bluefish | 0:c86a79c8b787 | 254 | msg.data[4] = ( right >> 8 ) & 0xFF; |
bluefish | 0:c86a79c8b787 | 255 | msg.data[5] = ( right >> 0 ) & 0xFF; |
bluefish | 7:77174a098e6f | 256 | msg.data[6] = ( cvd >> 8 ) & 0xFF; |
bluefish | 7:77174a098e6f | 257 | msg.data[7] = ( cvd >> 0 ) & 0xFF; |
bluefish | 0:c86a79c8b787 | 258 | can_driver.write( msg ); |
bluefish | 0:c86a79c8b787 | 259 | |
bluefish | 0:c86a79c8b787 | 260 | return; |
bluefish | 0:c86a79c8b787 | 261 | } |
bluefish | 0:c86a79c8b787 | 262 | |
bluefish | 0:c86a79c8b787 | 263 | void sensing_and_control(){ |
bluefish | 0:c86a79c8b787 | 264 | //LPF variables |
bluefish | 2:9c83de2d33ca | 265 | int16_t com = 0; |
bluefish | 2:9c83de2d33ca | 266 | int16_t comL = 0; |
bluefish | 4:72c26c07c251 | 267 | int16_t comR = 0; |
bluefish | 6:a5f674c2f262 | 268 | float if_angle = 0.0f; |
bluefish | 6:a5f674c2f262 | 269 | float if_angle_vel = 0.0f; |
bluefish | 6:a5f674c2f262 | 270 | |
bluefish | 6:a5f674c2f262 | 271 | // read sensor ( vehicle ) |
bluefish | 7:77174a098e6f | 272 | #ifdef USE_FIRST_IMU |
bluefish | 7:77174a098e6f | 273 | imu_vehicle.read6Axis( |
bluefish | 7:77174a098e6f | 274 | &sensor_vehicle.acc[0], &sensor_vehicle.acc[1], &sensor_vehicle.acc[2], |
bluefish | 7:77174a098e6f | 275 | &sensor_vehicle.gyro[0], &sensor_vehicle.gyro[1], &sensor_vehicle.gyro[2] |
bluefish | 0:c86a79c8b787 | 276 | ); |
bluefish | 7:77174a098e6f | 277 | sensor_vehicle.gyro[0] *= MPU9250G_DEG2RAD; |
bluefish | 7:77174a098e6f | 278 | sensor_vehicle.gyro[1] *= MPU9250G_DEG2RAD; |
bluefish | 7:77174a098e6f | 279 | sensor_vehicle.gyro[2] *= MPU9250G_DEG2RAD; |
bluefish | 7:77174a098e6f | 280 | // estimate angle |
bluefish | 7:77174a098e6f | 281 | proc_comp_filter( &cf_vehicle, PROCESS_INTERVAL_SEC, |
bluefish | 7:77174a098e6f | 282 | -( sensor_vehicle.acc[0] / sensor_vehicle.acc[2] ), sensor_vehicle.gyro[1], |
bluefish | 7:77174a098e6f | 283 | &posture.angle, &posture.angle_vel ); |
bluefish | 7:77174a098e6f | 284 | posture.angle -= control.C_offset_angle * MPU9250G_DEG2RAD; |
bluefish | 0:c86a79c8b787 | 285 | #endif |
bluefish | 0:c86a79c8b787 | 286 | |
bluefish | 7:77174a098e6f | 287 | #ifdef USE_SECOND_IMU |
bluefish | 7:77174a098e6f | 288 | // read sensor ( interface ) |
bluefish | 7:77174a098e6f | 289 | imu_interface.read6Axis( |
bluefish | 7:77174a098e6f | 290 | &sensor_interface.acc[0], &sensor_interface.acc[1], &sensor_interface.acc[2], |
bluefish | 7:77174a098e6f | 291 | &sensor_interface.gyro[0], &sensor_interface.gyro[1], &sensor_interface.gyro[2] |
bluefish | 7:77174a098e6f | 292 | ); |
bluefish | 7:77174a098e6f | 293 | sensor_interface.gyro[0] *= MPU9250G_DEG2RAD; |
bluefish | 7:77174a098e6f | 294 | sensor_interface.gyro[1] *= MPU9250G_DEG2RAD; |
bluefish | 7:77174a098e6f | 295 | sensor_interface.gyro[2] *= MPU9250G_DEG2RAD; |
bluefish | 7:77174a098e6f | 296 | // estimate angle |
bluefish | 7:77174a098e6f | 297 | proc_comp_filter( &cf_interface, PROCESS_INTERVAL_SEC, |
bluefish | 7:77174a098e6f | 298 | ( sensor_interface.acc[1] / sensor_interface.acc[2] ), sensor_interface.gyro[0], |
bluefish | 7:77174a098e6f | 299 | &if_angle, &if_angle_vel ); |
bluefish | 7:77174a098e6f | 300 | if_angle -= C_offset_angle * MPU9250G_DEG2RAD; |
bluefish | 7:77174a098e6f | 301 | if_angle /= MPU9250G_DEG2RAD; |
bluefish | 7:77174a098e6f | 302 | if_angle_vel /= MPU9250G_DEG2RAD; |
bluefish | 7:77174a098e6f | 303 | #endif |
bluefish | 6:a5f674c2f262 | 304 | |
bluefish | 0:c86a79c8b787 | 305 | #ifdef USE_SERIAL_DEBUG |
bluefish | 7:77174a098e6f | 306 | #ifdef USE_FIRST_IMU |
bluefish | 7:77174a098e6f | 307 | /* |
bluefish | 7:77174a098e6f | 308 | usb_serial.printf( "IMU 1:\t %f\t %f\t %f\t %f\t %f\t %f\n", |
bluefish | 7:77174a098e6f | 309 | sensor_vehicle.acc[0], sensor_vehicle.acc[1], sensor_vehicle.acc[2], |
bluefish | 7:77174a098e6f | 310 | sensor_vehicle.gyro[0], sensor_vehicle.gyro[1], sensor_vehicle.gyro[2] |
bluefish | 7:77174a098e6f | 311 | ); |
bluefish | 7:77174a098e6f | 312 | */ |
bluefish | 7:77174a098e6f | 313 | usb_serial.printf( "IMU1 \nANGLE :\t%f\nANGLE_VEL :\t%f\n", posture.angle, posture.angle_vel ); |
bluefish | 7:77174a098e6f | 314 | #endif |
bluefish | 7:77174a098e6f | 315 | #ifdef USE_SECOND_IMU |
bluefish | 7:77174a098e6f | 316 | /* |
bluefish | 7:77174a098e6f | 317 | usb_serial.printf( "IMU 2:%f\t %f\t %f\t %f\t %f\t %f\n", |
bluefish | 7:77174a098e6f | 318 | sensor_interface.acc[0], sensor_interface.acc[1], sensor_interface.acc[2], |
bluefish | 7:77174a098e6f | 319 | sensor_interface.gyro[0], sensor_interface.gyro[1], sensor_interface.gyro[2] |
bluefish | 7:77174a098e6f | 320 | ); |
bluefish | 7:77174a098e6f | 321 | */ |
bluefish | 7:77174a098e6f | 322 | usb_serial.printf( "IMU2 \nANGLE :\t%f\nANGLE_VEL :\t%f\n", if_angle,if_angle_vel ); |
bluefish | 7:77174a098e6f | 323 | #endif |
bluefish | 2:9c83de2d33ca | 324 | #endif |
bluefish | 0:c86a79c8b787 | 325 | |
bluefish | 0:c86a79c8b787 | 326 | // control motor |
bluefish | 2:9c83de2d33ca | 327 | com = 0; |
bluefish | 2:9c83de2d33ca | 328 | com += control.K_angle * posture.angle; |
bluefish | 2:9c83de2d33ca | 329 | com += control.K_angle_vel * posture.angle_vel; |
bluefish | 2:9c83de2d33ca | 330 | |
bluefish | 2:9c83de2d33ca | 331 | comL = com; |
bluefish | 2:9c83de2d33ca | 332 | comL += control.K_wheel * posture.wheel[0]; |
bluefish | 2:9c83de2d33ca | 333 | comL += control.K_wheel_vel * posture.wheel[0]; |
bluefish | 2:9c83de2d33ca | 334 | |
bluefish | 2:9c83de2d33ca | 335 | comR = com; |
bluefish | 2:9c83de2d33ca | 336 | comR += control.K_wheel * posture.wheel[1]; |
bluefish | 2:9c83de2d33ca | 337 | comR += control.K_wheel_vel * posture.wheel[1]; |
bluefish | 2:9c83de2d33ca | 338 | |
bluefish | 7:77174a098e6f | 339 | #ifdef USE_STEER_CONTROL |
bluefish | 7:77174a098e6f | 340 | float pad_x = 0.0f; |
bluefish | 7:77174a098e6f | 341 | pad_x = convPadJoystick( if_angle, C_dead_angle, C_max_angle ); |
bluefish | 7:77174a098e6f | 342 | comR += pad_x * control.K_rot_vel; |
bluefish | 7:77174a098e6f | 343 | comL -= pad_x * control.K_rot_vel; |
bluefish | 7:77174a098e6f | 344 | #endif |
bluefish | 7:77174a098e6f | 345 | |
bluefish | 7:77174a098e6f | 346 | #ifdef USE_MOTOR_CONTROL |
bluefish | 7:77174a098e6f | 347 | if( posture.angle < control.C_max_angle*MPU9250G_DEG2RAD && posture.angle > -control.C_max_angle*MPU9250G_DEG2RAD ){ |
bluefish | 7:77174a098e6f | 348 | controlMotor( comL, comR ); |
bluefish | 7:77174a098e6f | 349 | }else{ |
bluefish | 7:77174a098e6f | 350 | controlMotor( 0, 0 ); |
bluefish | 7:77174a098e6f | 351 | } |
bluefish | 7:77174a098e6f | 352 | #endif |
bluefish | 2:9c83de2d33ca | 353 | |
bluefish | 0:c86a79c8b787 | 354 | return; |
bluefish | 7:77174a098e6f | 355 | } |
bluefish | 7:77174a098e6f | 356 | |
bluefish | 7:77174a098e6f | 357 | float convPadJoystick( float input, const float val_deadzone, const float val_max ){ |
bluefish | 7:77174a098e6f | 358 | float ret = 0.0f; |
bluefish | 7:77174a098e6f | 359 | float k = 1.0f; |
bluefish | 7:77174a098e6f | 360 | if( val_deadzone != val_max ){ |
bluefish | 7:77174a098e6f | 361 | k = 1.0f / ( val_max - val_deadzone ); |
bluefish | 7:77174a098e6f | 362 | if( input < val_deadzone && input >-val_deadzone ){ |
bluefish | 7:77174a098e6f | 363 | ret = 0.0f; |
bluefish | 7:77174a098e6f | 364 | }else if( input > val_max ){ |
bluefish | 7:77174a098e6f | 365 | ret = 1.0f; |
bluefish | 7:77174a098e6f | 366 | }else if( input < -val_max ){ |
bluefish | 7:77174a098e6f | 367 | ret = -1.0f; |
bluefish | 7:77174a098e6f | 368 | }else{ |
bluefish | 7:77174a098e6f | 369 | ret = (input > 0.0f) ? (input - val_deadzone) : (input + val_deadzone); |
bluefish | 7:77174a098e6f | 370 | ret *= k; |
bluefish | 7:77174a098e6f | 371 | } |
bluefish | 7:77174a098e6f | 372 | } |
bluefish | 7:77174a098e6f | 373 | return ret; |
bluefish | 6:a5f674c2f262 | 374 | } |