Chris LU
2018/06/08
Diff: main.cpp
- Revision:
- 0:bf9bf4b7625f
diff -r 000000000000 -r bf9bf4b7625f main.cpp
--- /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