Chris LU
/
Self_Riding_Bicycle
2018/06/08
Diff: main.cpp
- Revision:
- 0:bf9bf4b7625f
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Fri Jun 08 14:11:49 2018 +0000 @@ -0,0 +1,403 @@ +#include "mbed.h" +#include "math.h" +#include "LSM9DS0.h" +#include "Mx28.h" +#include "Controller.h" +#include "SensorFusion.h" +#include "SystemConstant.h" + +// Dynamixel ************************************************************************************************************************************************** +#define Steering_SERVO_ID 3 +#define SERVO_ControlPin A2 // Control pin of buffer chip, NOTE: this does not matter becasue we are not using a half to full contorl buffer. +#define SERVO_SET_Baudrate 1000000 // Baud rate speed which the Dynamixel will be set too (1Mbps) +#define TxPin A0 +#define RxPin A1 +#define CW_LIMIT_ANGLE 0x001 // lowest clockwise angle is 1, as when set to 0 it set servo to wheel mode +#define CCW_LIMIT_ANGLE 0xFFF // Highest anit-clockwise angle is 0XFFF, as when set to 0 it set servo to wheel mode +DynamixelClass dynamixelClass(SERVO_SET_Baudrate,SERVO_ControlPin,TxPin,RxPin); +float deg_s = 0.0; +float left_motor_angle = 0.0f; +float right_motor_angle = 0.0f; + +// LSM9DS0 ***************************************************************************************************************************************************** +// sensor gain +#define Gyro_gain_x 0.002422966362920f +#define Gyro_gain_y 0.002422966362920f +#define Gyro_gain_z 0.002422966362920f // datasheet : 70 mdeg/digit +#define Acce_gain_x -0.002403878; // -9.81 / ( 3317.81 - (-764.05) ) +#define Acce_gain_y -0.002375119; // -9.81 / ( 3476.34 - (-676.806)) +#define Acce_gain_z -0.002454702; // -9.81 / ( 4423.63 - (285.406) ) +#define Magn_gain 0 + +// raw data register +int16_t Gyro_axis_data[3] = {0}; // X, Y, Z axis +int16_t Acce_axis_data[3] = {0}; // X, Y, Z axis + +// sensor filter correction data +float Gyro_axis_data_f[3]; +float Gyro_axis_data_f_old[3]; +float Gyro_scale[3]; // scale = (data - zero) * gain +float Gyro_axis_zero[3] = {51.38514174, 62.64907136, -20.82209189};//{52.6302,57.3614,-48.6806}; +//Gyro offset +//********************************************** +//41.5376344 -26.92864125 103.3949169 +//42.53079179 -27.71065494 97.0400782 +//43.54056696 -28.63734115 90.77419355 +//********************************************** + + +float Acce_axis_data_f[3]; +float Acce_axis_data_f_old[3]; +float Acce_scale[3]; // scale = (data - zero) * gain +float Acce_axis_zero[3] = {-754.5894428, -677.0645161, 413.4472141};//{-818.8824, -714.3789, 326.2993};//{-721.0150,-748.3353,394.9194}; +//Acce offset +//********************************************** +//-618.8191593 -639.3255132 4676.384164 +//-604.7047898 3395.289345 375.0957967 +//4676.57478 -700.4506354 416.9599218 +//********************************************** +LSM9DS0 sensor(SPI_MODE, PB_2, PB_1); // PB_13, PB_14, PB_15 + +// Useful constants ******************************************************************************************************************************************* +#define T 0.001 //sampling time +#define CNT2STR 1500 // 1500(ms) = 1.5(s) +// mbed function *********************************************************************************************************************************************** +Ticker timer1; +Ticker timer2; + +Serial USB(USBTX, USBRX); +Serial BT_Cmd(PC_12, PD_2); +Serial BT_Data(PC_10, PC_11); +Serial MCU(PB_6, PA_10); + +// Linear actuator ********************************************************************************************************************************************* +PwmOut pwm_l(D7); +PwmOut pwmn_l(D11); + +PwmOut pwm_r(D8); +PwmOut pwmn_r(A3); + +#define down_duty 1.0f +#define up_duty 0.0f +#define stop_duty 0.5f + +// ADC +AnalogIn RightActuator(PC_0); +AnalogIn LeftActuator(PC_1); + +// Functions *************************************************************************************************************************************************** +void init_sensor(void); +void read_sensor(void); +void timer1_interrupt(void); +void timer2_interrupt(void); +void uart_send(void); +void separate(void); + +// Variables *************************************************************************************************************************************************** +int i = 0; +int display[6] = {0,0,0,0,0,0}; +char num[14] = {254,255,0,0,0,0,0,0,0,0,0,0,0,0}; // char num[0] , num[1] : 2 start byte +char BTCmd = 0; +char MCU_Data_get = 0; +char MCU_Data_put = 0; +float velocity = 0.0f; +float velocity_old = 0.0f; +float speed = 0.0f; +float VelocityCmd = 0.0f; +float VelocityCmdOld = 0.0f; +float steer_out = 0.0f; +float steer_out_old = 0.0f; +bool start_control = 0; +float roll_sum = 0.0f; +float average = 0.0f; +int temp = 0; +// turning command +int count2straight = 0; +float roll_cmd = 0.0f; +float roll_old = 0.0f; +float roll = 0.0f; + +float steer_cmd = 0.0f; +float steer_old = 0.0f; +float steer = 0.0f; +// test tool +bool up = 0; +bool down = 0; +bool tim1 = 0; +int counter = 0; + +// Code ******************************************************************************************************************************************************** +// main ******************************************************************************************************************************************************** +int main() +{ + dynamixelClass.setMode(Steering_SERVO_ID, SERVO, CW_LIMIT_ANGLE, CCW_LIMIT_ANGLE); // set mode to SERVO and set angle limits + USB.baud(115200); + BT_Data.baud(57600); + BT_Cmd.baud(115200); + MCU.baud(115200); + + // actuator pwm initialize + pwm_l.period_us(50); + pwm_r.period_us(50); + // timer setting + timer1.attach_us(&timer1_interrupt, 1000);//4ms interrupt period + timer2.attach_us(&timer2_interrupt, 3000);//4.098ms interrupt period + + // initialize + sensor.begin(); + start_control = 0; + steering_angle = 0; + + while(1) { + // command from tablet + if(BT_Cmd.readable()) { + BTCmd = BT_Cmd.getc(); + switch(BTCmd) { + case 's': + start_control = 1; + roll_cmd = 2.0f/180.0f*pi; + count2straight = 0; + break; + case 'p': + start_control = 0; + steer_out = 0; + steer_rad = 0; + steer_rad_old = 0; + steering_angle = 0.0f; + u_1 = 0; + u_2 = 0; + u_3 = 0; + u_d = 0; + u = 0; + alpha_1 = 0; + alpha_2 = 0; + roll_err = 0; + VelocityCmd = 0; + break; + case 'f': + steer_out = 0; + break; + case 'r': + steer_out = steer_out + 2; +// up = 1; +// down = 0; + break; + case 'l': + steer_out = steer_out - 2; +// up = 0; +// down = 1; + break; + case 'm': // command of turning right + roll_cmd = 4.0f/180.0f*pi; + count2straight = 0; + break; + case 'n': // command of turning left + roll_cmd = 0.0f/180.0f*pi; + count2straight = 0; + break; + case 'a': + VelocityCmd = VelocityCmd + 0.0f; + break; + case 'b': + VelocityCmd = VelocityCmd - 0.0f; + break; + case 'c': + break; + case 'h': + VelocityCmd = 0; + steer_out = 0; + break; + default: + break; + } + BTCmd = 0; + } + } +}// main + +// timer1_interrupt ********************************************************************************************************************************************* +void timer1_interrupt(void) +{ + // MCU_UART /////////////////////////////////////////////////////////////////////////////////////////////////////////////// + if(VelocityCmd != VelocityCmdOld) { + if(MCU.writeable()) { + MCU_Data_put = VelocityCmd / 0.01953125f; + MCU.putc(MCU_Data_put); + } + VelocityCmdOld = VelocityCmd; + } + // speed data from another MCU + if(MCU.readable()) + { + MCU_Data_get = MCU.getc(); + tim1=!tim1; + } + speed = (float)MCU_Data_get * 0.01953125f; + // velocity check for auxiliary wheels /////////////////////////////////////////////////////////////////////////////////// + if(speed < 0.3f){ + counter = 0; + pwm_l.write(down_duty); + pwm_r.write(down_duty); + } + if(speed > 0.3f){ + counter++; + pwm_l.write(up_duty); + pwm_r.write(up_duty); + if(counter >= 16000){ + counter = 16000; + pwm_l.write(stop_duty); + pwm_r.write(stop_duty); + } + } + // if(up ==1){ +// pwm_l.write(up_duty); +// pwm_r.write(up_duty); +// } +// if(down ==1){ +// pwm_l.write(down_duty); +// pwm_r.write(down_duty); +// } + + TIM1->CCER |= 0x4; + TIM1->CCER |= 0x40; + velocity = 1; + // IMU Read //////////////////////////////////////////////////////////////////////////////////////////////////////////////// + read_sensor(); + Acce_axis_data_f[0] = lpf(Acce_axis_data[0],Acce_axis_data_f_old[0],18); + Acce_axis_data_f_old[0] = Acce_axis_data_f[0]; + Acce_axis_data_f[1] = lpf(Acce_axis_data[1],Acce_axis_data_f_old[1],18); + Acce_axis_data_f_old[1] = Acce_axis_data_f[1]; + Acce_axis_data_f[2] = lpf(Acce_axis_data[2],Acce_axis_data_f_old[2],18); + Acce_axis_data_f_old[2] = Acce_axis_data_f[2]; + + Gyro_axis_data_f[0] = lpf(Gyro_axis_data[0],Gyro_axis_data_f_old[0],18); + Gyro_axis_data_f_old[0] = Gyro_axis_data_f[0]; + Gyro_axis_data_f[1] = lpf(Gyro_axis_data[1],Gyro_axis_data_f_old[1],18); + Gyro_axis_data_f_old[1] = Gyro_axis_data_f[1]; + Gyro_axis_data_f[2] = lpf(Gyro_axis_data[2],Gyro_axis_data_f_old[2],18); + Gyro_axis_data_f_old[2] = Gyro_axis_data_f[2]; + + Acce_scale[0] = ((Acce_axis_data_f[0]-Acce_axis_zero[0]))*Acce_gain_x; + Acce_scale[1] = ((Acce_axis_data_f[1]-Acce_axis_zero[1]))*Acce_gain_y; + Acce_scale[2] = ((Acce_axis_data_f[2]-Acce_axis_zero[2]))*Acce_gain_z; + + Gyro_scale[0] = ((Gyro_axis_data_f[0]-Gyro_axis_zero[0]))*Gyro_gain_x; + Gyro_scale[1] = ((Gyro_axis_data_f[1]-Gyro_axis_zero[1]))*Gyro_gain_y; + Gyro_scale[2] = ((Gyro_axis_data_f[2]-Gyro_axis_zero[2]))*Gyro_gain_z; + + //Centrifugal Acceleration Compensation //////////////////////////////////////////////////////////////////////////////////// + /* + Acx = Ac*sin(e) = YawRate*YawRate*l_rs + Acx = Ac*sin(e)*cos(roll_angle) = YawRate*speed*cos(roll_angle) + Acx = Ac*sin(e)*sin(roll_angle) = YawRate*speed*sin(roll_angle) + */ + Ac[0] = l_rs * Gyro_scale[2] * Gyro_scale[2]; + Ac[1] = (-1.0) * velocity * Gyro_scale[2] * cos(roll_angle); + Ac[2] = (-1.0) * velocity * Gyro_scale[2] * sin(roll_angle); + Acce_scale[0] = Acce_scale[0] - Ac[0]; + Acce_scale[1] = Acce_scale[1] - Ac[1]; + Acce_scale[2] = Acce_scale[2] - Ac[2]; + // do sensor fusion ///////////////////////////////////////////////////////////////////////////////////////////////////////// + roll_fusion(Acce_scale[0],Acce_scale[1],Acce_scale[2],Gyro_scale[2],Gyro_scale[0],5);// alpha = 3 represents the best behavior for robot bike + droll_angle = lpf(Gyro_scale[0], droll_angle_old, 62.8); // 62.8 = pi * 20 + droll_angle_old = droll_angle; + droll_angle = droll_angle + 0.5f/180.0f*pi; + // bias cancelation +// roll_angle = roll_angle - 2.2f/180.0f*pi; + // roll angle of turning command //////////////////////////////////////////////////////////////////////////////////////////// + roll_ref = lpf(roll_cmd, roll_old, 3.1415f); + roll_old = roll_ref; + // steering angle of turning command //////////////////////////////////////////////////////////////////////////////////////// + /* + -L*g + steer_ss = --------------- * roll_ss + V*V*cos(lammda) + */ + steer_ref = -L*g/velocity/velocity/cos(lammda)*roll_ref; + // controller //////////////////////////////////////////////////////////////////////////////////////////////////////////////// + if(start_control == 0) { + deg_s = (180.0f + steer_out)*4096.0f/360.0f; + } + if(start_control == 1) { + + // lpf for velocity, cut-off freq. = 3Hz = 18.85 (rad/s) + velocity = lpf(velocity, velocity_old, 18.85); + velocity_old = velocity; + + controller(velocity); + steer_angle(u, velocity); + + //Steer output + deg_s = (180.0f - steering_angle)*4096.0f/360.0f; + // count to turn stright + if(count2straight < CNT2STR) count2straight++; + if(count2straight == CNT2STR) roll_cmd = 2.0f/180.0f*pi; + } + // send data by UART ////////////////////////////////////////////////////////////////////////////////////////////////////////// + uart_send(); +}// timer1_interrupt + +// timer1_interrupt ********************************************************************************************************************************************* +void timer2_interrupt(void) +{ + dynamixelClass.servo(Steering_SERVO_ID,deg_s,1000); +}// timer2_interrupt + +// read_sensor ************************************************************************************************************************************************** +void read_sensor(void) +{ + // sensor raw data + Gyro_axis_data[0] = sensor.readRawGyroX(); + Gyro_axis_data[1] = sensor.readRawGyroY(); + Gyro_axis_data[2] = sensor.readRawGyroZ(); + + Acce_axis_data[0] = sensor.readRawAccelX(); + Acce_axis_data[1] = sensor.readRawAccelY(); + Acce_axis_data[2] = sensor.readRawAccelZ(); +}// read_sensor + +// uart_send **************************************************************************************************************************************************** +void uart_send(void) +{ + // uart send data + display[0] = roll_angle/pi*180.0f*100.0f; + display[1] = droll_angle/pi*180.0f*100.0f; + display[2] = speed*100; + display[3] = steering_angle*100; + display[4] = u_1*100; + display[5] = u_2*100; + + separate(); + + int j = 0; + int k = 1; + while(k) { + if(BT_Data.writeable()) { + BT_Data.putc(num[i]); + i++; + j++; + } + if(j>1) {// send 2 bytes + k = 0; + j = 0; + } + } + if(i>13) i = 0; +}// uart_send + +//separate **************************************************************************************************************************************************** +void separate(void) +{ + num[2] = display[0] >> 8; // HSB(8bit)資料存入陣列 + num[3] = display[0] & 0x00ff; // LSB(8bit)資料存入陣列 + num[4] = display[1] >> 8; + num[5] = display[1] & 0x00ff; + num[6] = display[2] >> 8; + num[7] = display[2] & 0x00ff; + num[8] = display[3] >> 8; + num[9] = display[3] & 0x00ff; + num[10] = display[4] >> 8; + num[11] = display[4] & 0x00ff; + num[12] = display[5] >> 8; + num[13] = display[5] & 0x00ff; +}//separate \ No newline at end of file