Shimon AJISAKA / Mbed 2 deprecated InvertedPendulum2017

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?

UserRevisionLine numberNew 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 }