Chris LU
code of robot bike
Dependencies: SDFileSystem mbed
Fork of
Robot_Bicycle
by 
Chris LU
Diff: main.cpp
- Revision:
- 4:b0967990e390
- Parent:
- 3:197b748a397a
- Child:
- 5:2290732b2782
--- a/main.cpp Wed Jun 22 04:35:18 2016 +0000
+++ b/main.cpp Wed Jul 06 06:54:15 2016 +0000
@@ -27,8 +27,8 @@
// |PH_0 | PA_1 A1| ||ls |PB_5 D4 | PB_13|CK2
// |PH_1 | PA_4 A2| ||le |PB_3 D3 | AGND |
// |VBAT | PB_0 A3| ||lw |PA_10 D2 | PC_4 |
-// |PC_2 | PC_1 A4| ||TX2|PA_2 D1 | NC |
-// |PC_3 | PC_0 A5| ||RX2|PA_3 D0 | NC |
+//out|PC_2 | PC_1 A4| ||TX2|PA_2 D1 | NC |
+//out|PC_3 | PC_0 A5|IR ||RX2|PA_3 D0 | NC |
// |---------------| || |-----------------|
//************************************************//
@@ -44,9 +44,12 @@
Ticker timer2;
Timeout timeout;
+DigitalOut pin_pc2(PC_2);
+DigitalOut pin_pc3(PC_3);
+
Serial BT(PC_6, PA_12);
-SPI spi3(PC_12, PC_11, PC_10);
+SPI SD_card(PC_12, PC_11, PC_10);
//**** Variables from Arduino ****//
int counter = 0;
@@ -85,8 +88,11 @@
void attimeout(void);
int main() {
+ pin_pc2 = pin_pc3 = 0;
+
setupServo();
- setup_spi();
+ pc.baud(115200);
+ setup_spi_sensor();
init_Sensors();
BT.baud(115200);
@@ -95,11 +101,13 @@
timer1.attach_us(&timer1_interrupt, 4000);//4.0ms interrupt period (250 Hz)
timer2.attach_us(&timer2_interrupt, 4098);//4.098ms interrupt period (244 Hz)
- spi3.format(8, 3);
+ SD_card.format(8, 3);
+// fprintf_data(0);
pc.printf("System ready.\r\n");
reset_pos();
lookuptable_steering(HANDLE_START);
+// lookuptable_steering(0);
while(!interrupt)
{
@@ -108,7 +116,8 @@
BTCharR = BT.getc();
switch(BTCharR)
{
- case 's': lookuptable_pedaling(FIRST_POS);
+ case 's': reset_offset();
+ lookuptable_pedaling(FIRST_POS);
lookuptable_steering(HANDLE_STRAIGHT);
wait(1.5f);
pedal_state = 1; sys_state = L_PD; gamma_ref = 0.0;
@@ -126,17 +135,183 @@
BTCharR = 0;
}
+ sensorG_read_3axis();
+ sensorX_read_3axis();
+
+ get_9axis_scale();
+ get_9axis_data(pedal_state);
+
L_inver = 0.0063f * V_meas - 0.005769f;
}
}
void timer1_interrupt(void)
{
+// pin_pc2 = !pin_pc2;
V_meas = (analog_value.read() * 3.3f) * IR_FILT_CONST + V_meas * (1.0f - IR_FILT_CONST);
+
+// pc.printf("%d\t%d\t%d\r\n", filted_sensor_data(INDEX_ACCE_X, 1.8), filted_sensor_data(INDEX_ACCE_Y, 1.8), filted_sensor_data(INDEX_ACCE_Z, 1.8));
+
+ roll_fusion(axm,aym,azm,u3,u1,1);
+ x1_hat_estimat(axm,aym,azm,u3,u2,Alpha);
+ m_x1_hat(mx,my,mz,u3,u2,Alpha);
+ m_x2_hat(mx,my,mz,u3,u1,Alpha);
+
+ if(cosroll != 0)
+ {
+ droll_angle = lpf(u1, droll_angle_old, 62.8);
+ droll_angle_old = droll_angle;
+ dyaw_angle = lpf(u3/cosroll, dyaw_angle_old, 3.1416);
+ dyaw_angle_old = dyaw_angle;
+// m_yaw_fusion();
+
+ ///cut off
+ if(m_sinyaw <= -1.0f)m_sinyaw = -1.0f;
+ else if(m_sinyaw >= 1.0f)m_sinyaw = 1.0f;
+ else ;
+ if(m_cosyaw <= -1.0f)m_cosyaw = -1.0f;
+ else if(m_cosyaw >= 1.0f)m_cosyaw = 1.0f;
+ else ;
+
+ ///Controller
+ if(sys_state == L_PD)
+ {
+ calc_PD(K_l, 0.0);
+ calc_Gamma(u,15,b_h);
+ gamma_rad = gamma_rad + L_PD_OFFSET;
+
+ dphi_hat = 0.0;
+ phi_hat = 0.0;
+ yaw_angle = yaw_angle_old + dyaw_angle * sample_time;
+ yaw_angle_old = yaw_angle;
+ }
+ else if(sys_state == M_PD)
+ {
+ calc_PD(K_m, 0.0);
+ calc_Gamma(u,15,b_h);
+
+ dphi_hat = 0.0;
+ phi_hat = 0.0;
+ yaw_angle = yaw_angle_old + dyaw_angle * sample_time;
+ yaw_angle_old = yaw_angle;
+
+ count2ztc_m++;
+ if(count2ztc_m >= CNT2ZTCm){sys_state = M_ZTC;}
+ }
+ else if(sys_state == M_ZTC)
+ {
+ if(count2straight < CNT2STRT){count2straight++;}
+ else{gamma_ref = 0.0; roll_ref = 0.0;}
+ calc_PD(K_m, phi_hat);
+ calc_Phi(K_mphi);
+ calc_Gamma(u,15,b_h);
+
+ yaw_angle = yaw_angle_old + dyaw_angle * sample_time;
+ yaw_angle_old = yaw_angle;
+ }
+ else if(sys_state == H_PD)
+ {
+ calc_PD(K_h,0.0);
+ calc_Gamma(u,15,b_h);
+
+ dphi_hat = 0.0;
+ phi_hat = 0.0;
+ yaw_angle = yaw_angle_old + dyaw_angle * sample_time;
+ yaw_angle_old = yaw_angle;
+
+// count2ztc_h++;
+ if(count2ztc_h >= CNT2ZTCh){sys_state = H_ZTC;}
+ }
+ else if(sys_state == H_ZTC)
+ {
+ if(count2straight < CNT2STRT){count2straight++;}
+ else{gamma_ref = 0.0; roll_ref = 0.0;}
+ calc_PD(K_h, phi_hat);
+ calc_Phi(K_hphi);
+ calc_Gamma(u,15,b_h);
+
+ yaw_angle = yaw_angle_old + dyaw_angle * sample_time;
+ yaw_angle_old = yaw_angle;
+ }
+ else
+ {
+ u = 0.0;
+ calc_Gamma(u,15,b_h);
+ dphi_hat = 0.0;
+ phi_hat = 0.0;
+ yaw_angle = 0.0;
+ yaw_angle_old = 0.0;
+ }
+
+ ///Anti-Windup
+ anti_wdup();
+
+// calc_Gamma(u,15,b_h);
+ if(gamma_rad > 0.349f)gamma_rad = 0.349f;
+ else if(gamma_rad < -0.349f)gamma_rad = -0.349f;
+ else ;
+// printf("%d Roll:%9.6f dRoll:%9.6f u:%9.6f phi_hat:%9.6f gamma:%9.6f\r\n", sys_state, roll_angle, droll_angle, u, phi_hat, gamma_rad);
+
+ ///Show results or Send datas
+ gamma_degree = (short int)(gamma_rad*114.59f);
+ if(gamma_degree > 40)gamma_degree = 40;
+ else if(gamma_degree < -40)gamma_degree = -40;
+ else ;
+
+ ///Steering
+ if(s == 1)
+ {
+ if(state_count > COUN2_HANDLE_START)
+ {
+ lookuptable_steering( gamma_degree );
+ }
+ else
+ {
+ lookuptable_steering( gamma_degree + HANDLE_START_BAL);
+ }
+ }
+ else
+ {
+ if(c == FIRST_POS)
+ {
+ if(state_change > 0)
+ {
+ lookuptable_steering(HANDLE_STOP);
+ }
+ else
+ {
+ lookuptable_steering(HANDLE_START);
+// lookuptable_steering(HANDLE_STRAIGHT);
+// lookuptable_steering( gamma_degree );
+ }
+ }
+ else lookuptable_steering( gamma_degree + HANDLE_START_BAL);
+ }
+
+ ///print file
+ if(sys_state >= 1)
+ {
+ count_isr++;
+ T_total = (float)sample_time*count_isr;
+
+ ///Integrate Ax to get Vx
+ Vx = Vx_old - axm_f*sample_time;
+ Vx_old = Vx;
+// if(Vx >= v_low && sys_state < M_PD){pedal_state = 2; sys_state = M_PD; gamma_ref = 0.0;}
+
+// fprintf_data(1);
+ }
+ }
+ else
+ {
+ roll_fusion(0,0,0,0,0,1);
+ gamma_degree = 0;
+ }
}
void timer2_interrupt(void)
{
+// pin_pc3 = !pin_pc3;
if(s == 1) // bluetooth start
{
i ++;
@@ -216,7 +391,7 @@
}
else
{
- reset_pos();
+// reset_pos();
}
}
else