main.cpp

00001 #include "mbed.h"
00002 #include "math.h"
00003 #include "LSM9DS0.h"
00004 #include "Mx28.h"
00005 #include "Controller.h"
00006 #include "SensorFusion.h"
00007 #include "SystemConstant.h"
00008 
00009 // Dynamixel **************************************************************************************************************************************************
00010 #define Steering_SERVO_ID           3
00011 #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.
00012 #define SERVO_SET_Baudrate          1000000  // Baud rate speed which the Dynamixel will be set too (1Mbps)
00013 #define TxPin                       A0
00014 #define RxPin                       A1
00015 #define CW_LIMIT_ANGLE              0x001    // lowest clockwise angle is 1, as when set to 0 it set servo to wheel mode
00016 #define CCW_LIMIT_ANGLE             0xFFF    // Highest anit-clockwise angle is 0XFFF, as when set to 0 it set servo to wheel mode
00017 DynamixelClass dynamixelClass(SERVO_SET_Baudrate,SERVO_ControlPin,TxPin,RxPin);
00018 float deg_s = 0.0;
00019 float left_motor_angle = 0.0f;
00020 float right_motor_angle = 0.0f;
00021 
00022 // LSM9DS0 *****************************************************************************************************************************************************
00023 // sensor gain
00024 #define Gyro_gain_x   0.002422966362920f
00025 #define Gyro_gain_y   0.002422966362920f
00026 #define Gyro_gain_z   0.002422966362920f            // datasheet : 70 mdeg/digit
00027 #define Acce_gain_x   -0.002403878;                 // -9.81 / ( 3317.81 - (-764.05) )
00028 #define Acce_gain_y   -0.002375119;                 // -9.81 / ( 3476.34 - (-676.806))
00029 #define Acce_gain_z   -0.002454702;                 // -9.81 / ( 4423.63 - (285.406) )
00030 #define Magn_gain     0
00031 
00032 // raw data register
00033 int16_t Gyro_axis_data[3] = {0};     // X, Y, Z axis
00034 int16_t Acce_axis_data[3] = {0};     // X, Y, Z axis
00035 
00036 // sensor filter correction data
00037 float Gyro_axis_data_f[3];
00038 float Gyro_axis_data_f_old[3];
00039 float Gyro_scale[3];                          // scale = (data - zero) * gain
00040 float Gyro_axis_zero[3] = {51.38514174, 62.64907136, -20.82209189};//{52.6302,57.3614,-48.6806};
00041 //Gyro offset
00042 //**********************************************
00043 //41.5376344    -26.92864125    103.3949169
00044 //42.53079179   -27.71065494    97.0400782
00045 //43.54056696   -28.63734115    90.77419355
00046 //**********************************************
00047 
00048 
00049 float Acce_axis_data_f[3];
00050 float Acce_axis_data_f_old[3];
00051 float Acce_scale[3];                          // scale = (data - zero) * gain
00052 float Acce_axis_zero[3] = {-754.5894428, -677.0645161, 413.4472141};//{-818.8824, -714.3789, 326.2993};//{-721.0150,-748.3353,394.9194};
00053 //Acce offset
00054 //**********************************************
00055 //-618.8191593    -639.3255132    4676.384164
00056 //-604.7047898    3395.289345     375.0957967
00057 //4676.57478     -700.4506354   416.9599218
00058 //**********************************************
00059 LSM9DS0 sensor(SPI_MODE, PB_2, PB_1);    // PB_13, PB_14, PB_15
00060 
00061 // Useful constants  *******************************************************************************************************************************************
00062 #define T 0.001 //sampling time
00063 #define CNT2STR 1500 // 1500(ms) = 1.5(s)
00064 // mbed function ***********************************************************************************************************************************************
00065 Ticker timer1;
00066 Ticker timer2;
00067 
00068 Serial USB(USBTX, USBRX);
00069 Serial BT_Cmd(PC_12, PD_2);
00070 Serial BT_Data(PC_10, PC_11);
00071 Serial MCU(PB_6, PA_10);
00072 
00073 // Linear actuator *********************************************************************************************************************************************
00074 PwmOut pwm_l(D7);
00075 PwmOut pwmn_l(D11);
00076 
00077 PwmOut pwm_r(D8);
00078 PwmOut pwmn_r(A3);
00079 
00080 #define down_duty 1.0f
00081 #define up_duty   0.0f
00082 #define stop_duty 0.5f
00083 
00084 // ADC
00085 AnalogIn RightActuator(PC_0);
00086 AnalogIn LeftActuator(PC_1);
00087 
00088 // Functions ***************************************************************************************************************************************************
00089 void init_sensor(void);
00090 void read_sensor(void);
00091 void timer1_interrupt(void);
00092 void timer2_interrupt(void);
00093 void uart_send(void);
00094 void separate(void);
00095 
00096 // Variables ***************************************************************************************************************************************************
00097 int i = 0;
00098 int display[6] = {0,0,0,0,0,0};
00099 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
00100 char BTCmd = 0;
00101 char MCU_Data_get = 0;
00102 char MCU_Data_put = 0;
00103 float velocity = 0.0f;
00104 float velocity_old = 0.0f;
00105 float speed = 0.0f;
00106 float VelocityCmd = 0.0f;
00107 float VelocityCmdOld = 0.0f;
00108 float steer_out = 0.0f;
00109 float steer_out_old = 0.0f;
00110 bool start_control = 0;
00111 float roll_sum = 0.0f;
00112 float average = 0.0f;
00113 int temp = 0;
00114 // turning command
00115 int count2straight = 0;
00116 float roll_cmd = 0.0f;
00117 float roll_old = 0.0f;
00118 float roll = 0.0f;
00119 
00120 float steer_cmd = 0.0f;
00121 float steer_old = 0.0f;
00122 float steer = 0.0f;
00123 // test tool
00124 bool up = 0;
00125 bool down = 0;
00126 bool tim1 = 0;
00127 int counter = 0;
00128 
00129 // Code ********************************************************************************************************************************************************
00130 // main ********************************************************************************************************************************************************
00131 int main()
00132 {
00133     dynamixelClass.setMode(Steering_SERVO_ID, SERVO, CW_LIMIT_ANGLE, CCW_LIMIT_ANGLE);    // set mode to SERVO and set angle limits
00134     USB.baud(115200);
00135     BT_Data.baud(57600);
00136     BT_Cmd.baud(115200);
00137     MCU.baud(115200);
00138     
00139     // actuator pwm initialize
00140     pwm_l.period_us(50);    
00141     pwm_r.period_us(50);    
00142     // timer setting
00143     timer1.attach_us(&timer1_interrupt, 1000);//4ms interrupt period
00144     timer2.attach_us(&timer2_interrupt, 3000);//4.098ms interrupt period
00145     
00146     // initialize
00147     sensor.begin();
00148     start_control = 0;
00149     steering_angle = 0;
00150     
00151     while(1) {
00152         // command from tablet
00153         if(BT_Cmd.readable()) {
00154             BTCmd = BT_Cmd.getc();
00155             switch(BTCmd) {
00156                 case 's':
00157                     start_control = 1;
00158                     roll_cmd = 2.0f/180.0f*pi;
00159                     count2straight = 0;
00160                     break;
00161                 case 'p':
00162                     start_control = 0;
00163                     steer_out = 0;
00164                     steer_rad = 0;
00165                     steer_rad_old = 0;
00166                     steering_angle = 0.0f;
00167                     u_1 = 0;
00168                     u_2 = 0;
00169                     u_3 = 0;
00170                     u_d = 0;
00171                     u = 0;
00172                     alpha_1 = 0;
00173                     alpha_2 = 0;
00174                     roll_err = 0;
00175                     VelocityCmd = 0;
00176                     break;
00177                 case 'f':
00178                     steer_out = 0;
00179                     break;
00180                 case 'r':
00181                     steer_out = steer_out + 2;
00182 //                    up = 1;
00183 //                    down = 0;
00184                     break;
00185                 case 'l':
00186                     steer_out = steer_out - 2;
00187 //                    up = 0;
00188 //                    down = 1;
00189                     break;
00190                 case 'm': // command of turning right
00191                     roll_cmd = 4.0f/180.0f*pi;
00192                     count2straight = 0;
00193                     break;
00194                 case 'n': // command of turning left
00195                     roll_cmd = 0.0f/180.0f*pi;
00196                     count2straight = 0;
00197                     break;
00198                 case 'a':
00199                     VelocityCmd = VelocityCmd + 0.0f;
00200                     break;
00201                 case 'b':
00202                     VelocityCmd = VelocityCmd - 0.0f;
00203                     break;
00204                 case 'c':
00205                     break;
00206                 case 'h':
00207                     VelocityCmd = 0;
00208                     steer_out = 0;
00209                     break;
00210                 default:
00211                     break;
00212             }
00213             BTCmd = 0;
00214         }
00215     }
00216 }// main
00217 
00218 // timer1_interrupt *********************************************************************************************************************************************
00219 void timer1_interrupt(void)
00220 {   
00221     // MCU_UART ///////////////////////////////////////////////////////////////////////////////////////////////////////////////
00222     if(VelocityCmd != VelocityCmdOld) {
00223         if(MCU.writeable()) {
00224             MCU_Data_put = VelocityCmd / 0.01953125f;
00225             MCU.putc(MCU_Data_put);
00226         }
00227         VelocityCmdOld = VelocityCmd;
00228     }
00229     // speed data from another MCU
00230     if(MCU.readable())
00231     {
00232         MCU_Data_get = MCU.getc();
00233         tim1=!tim1;
00234     }
00235     speed = (float)MCU_Data_get * 0.01953125f;
00236     // velocity check for auxiliary wheels ///////////////////////////////////////////////////////////////////////////////////
00237     if(speed < 0.3f){
00238         counter = 0;
00239         pwm_l.write(down_duty);
00240         pwm_r.write(down_duty);
00241     }
00242     if(speed > 0.3f){
00243         counter++;
00244         pwm_l.write(up_duty);
00245         pwm_r.write(up_duty);
00246         if(counter >= 16000){
00247             counter = 16000;
00248             pwm_l.write(stop_duty);
00249             pwm_r.write(stop_duty);
00250         }
00251     }
00252    // if(up ==1){
00253 //        pwm_l.write(up_duty);
00254 //        pwm_r.write(up_duty);
00255 //    }
00256 //    if(down ==1){
00257 //        pwm_l.write(down_duty);
00258 //        pwm_r.write(down_duty);
00259 //    }
00260     
00261     TIM1->CCER |= 0x4;
00262     TIM1->CCER |= 0x40;
00263     velocity = 1;
00264     // IMU Read ////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00265     read_sensor();
00266     Acce_axis_data_f[0] = lpf(Acce_axis_data[0],Acce_axis_data_f_old[0],18);
00267     Acce_axis_data_f_old[0] = Acce_axis_data_f[0];
00268     Acce_axis_data_f[1] = lpf(Acce_axis_data[1],Acce_axis_data_f_old[1],18);
00269     Acce_axis_data_f_old[1] = Acce_axis_data_f[1];
00270     Acce_axis_data_f[2] = lpf(Acce_axis_data[2],Acce_axis_data_f_old[2],18);
00271     Acce_axis_data_f_old[2] = Acce_axis_data_f[2];
00272 
00273     Gyro_axis_data_f[0] = lpf(Gyro_axis_data[0],Gyro_axis_data_f_old[0],18);
00274     Gyro_axis_data_f_old[0] = Gyro_axis_data_f[0];
00275     Gyro_axis_data_f[1] = lpf(Gyro_axis_data[1],Gyro_axis_data_f_old[1],18);
00276     Gyro_axis_data_f_old[1] = Gyro_axis_data_f[1];
00277     Gyro_axis_data_f[2] = lpf(Gyro_axis_data[2],Gyro_axis_data_f_old[2],18);
00278     Gyro_axis_data_f_old[2] = Gyro_axis_data_f[2];
00279 
00280     Acce_scale[0] = ((Acce_axis_data_f[0]-Acce_axis_zero[0]))*Acce_gain_x;
00281     Acce_scale[1] = ((Acce_axis_data_f[1]-Acce_axis_zero[1]))*Acce_gain_y;
00282     Acce_scale[2] = ((Acce_axis_data_f[2]-Acce_axis_zero[2]))*Acce_gain_z;
00283 
00284     Gyro_scale[0] = ((Gyro_axis_data_f[0]-Gyro_axis_zero[0]))*Gyro_gain_x;
00285     Gyro_scale[1] = ((Gyro_axis_data_f[1]-Gyro_axis_zero[1]))*Gyro_gain_y;
00286     Gyro_scale[2] = ((Gyro_axis_data_f[2]-Gyro_axis_zero[2]))*Gyro_gain_z;
00287 
00288     //Centrifugal Acceleration Compensation ////////////////////////////////////////////////////////////////////////////////////
00289     /*
00290                                 Acx = Ac*sin(e) = YawRate*YawRate*l_rs
00291                                 Acx = Ac*sin(e)*cos(roll_angle) = YawRate*speed*cos(roll_angle)
00292                                 Acx = Ac*sin(e)*sin(roll_angle) = YawRate*speed*sin(roll_angle)
00293     */
00294     Ac[0] = l_rs * Gyro_scale[2] * Gyro_scale[2];
00295     Ac[1] = (-1.0) * velocity * Gyro_scale[2] * cos(roll_angle);
00296     Ac[2] = (-1.0) * velocity * Gyro_scale[2] * sin(roll_angle);
00297     Acce_scale[0] = Acce_scale[0] - Ac[0];
00298     Acce_scale[1] = Acce_scale[1] - Ac[1];
00299     Acce_scale[2] = Acce_scale[2] - Ac[2];
00300     // do sensor fusion /////////////////////////////////////////////////////////////////////////////////////////////////////////
00301     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
00302     droll_angle = lpf(Gyro_scale[0], droll_angle_old, 62.8);  // 62.8 = pi * 20
00303     droll_angle_old = droll_angle;
00304     droll_angle = droll_angle + 0.5f/180.0f*pi;
00305     // bias cancelation
00306 //    roll_angle = roll_angle - 2.2f/180.0f*pi;
00307     // roll angle of turning command ////////////////////////////////////////////////////////////////////////////////////////////
00308     roll_ref = lpf(roll_cmd, roll_old, 3.1415f);
00309     roll_old = roll_ref;
00310     // steering angle of turning command ////////////////////////////////////////////////////////////////////////////////////////
00311     /*
00312                                                         -L*g
00313                                         steer_ss = ---------------  * roll_ss
00314                                                      V*V*cos(lammda)
00315     */
00316     steer_ref = -L*g/velocity/velocity/cos(lammda)*roll_ref;
00317     // controller ////////////////////////////////////////////////////////////////////////////////////////////////////////////////
00318     if(start_control == 0) {
00319         deg_s = (180.0f + steer_out)*4096.0f/360.0f;
00320     }
00321     if(start_control == 1) {
00322         
00323         // lpf for velocity, cut-off freq. = 3Hz = 18.85 (rad/s)
00324         velocity = lpf(velocity, velocity_old, 18.85);
00325         velocity_old = velocity;
00326         
00327         controller(velocity);
00328         steer_angle(u, velocity);
00329 
00330         //Steer output
00331         deg_s = (180.0f - steering_angle)*4096.0f/360.0f;
00332         // count to turn stright
00333         if(count2straight < CNT2STR)  count2straight++;
00334         if(count2straight == CNT2STR) roll_cmd = 2.0f/180.0f*pi;
00335     }
00336     // send data by UART //////////////////////////////////////////////////////////////////////////////////////////////////////////
00337     uart_send();
00338 }// timer1_interrupt
00339 
00340 // timer1_interrupt *********************************************************************************************************************************************
00341 void timer2_interrupt(void)
00342 {
00343  dynamixelClass.servo(Steering_SERVO_ID,deg_s,1000);
00344 }// timer2_interrupt
00345 
00346 // read_sensor **************************************************************************************************************************************************
00347 void read_sensor(void)
00348 {
00349     // sensor raw data
00350     Gyro_axis_data[0] = sensor.readRawGyroX();
00351     Gyro_axis_data[1] = sensor.readRawGyroY();
00352     Gyro_axis_data[2] = sensor.readRawGyroZ();
00353 
00354     Acce_axis_data[0] = sensor.readRawAccelX();
00355     Acce_axis_data[1] = sensor.readRawAccelY();
00356     Acce_axis_data[2] = sensor.readRawAccelZ();
00357 }// read_sensor
00358 
00359 // uart_send ****************************************************************************************************************************************************
00360 void uart_send(void)
00361 {
00362     // uart send data
00363     display[0] = roll_angle/pi*180.0f*100.0f;
00364     display[1] = droll_angle/pi*180.0f*100.0f;
00365     display[2] = speed*100;
00366     display[3] = steering_angle*100;
00367     display[4] = u_1*100;
00368     display[5] = u_2*100;
00369 
00370     separate();
00371 
00372     int j = 0;
00373     int k = 1;
00374     while(k)    {
00375         if(BT_Data.writeable()) {
00376             BT_Data.putc(num[i]);
00377             i++;
00378             j++;
00379         }
00380         if(j>1) {// send 2 bytes
00381             k = 0;
00382             j = 0;
00383         }
00384     }
00385     if(i>13) i = 0;
00386 }// uart_send
00387 
00388 //separate  ****************************************************************************************************************************************************
00389 void separate(void)
00390 {
00391     num[2] = display[0] >> 8;           // HSB(8bit)資料存入陣列
00392     num[3] = display[0] & 0x00ff;       // LSB(8bit)資料存入陣列
00393     num[4] = display[1] >> 8;
00394     num[5] = display[1] & 0x00ff;
00395     num[6] = display[2] >> 8;
00396     num[7] = display[2] & 0x00ff;
00397     num[8] = display[3] >> 8;
00398     num[9] = display[3] & 0x00ff;
00399     num[10] = display[4] >> 8;
00400     num[11] = display[4] & 0x00ff;
00401     num[12] = display[5] >> 8;
00402     num[13] = display[5] & 0x00ff;
00403 }//separate