Chris LU / Mbed 2 deprecated Nucleo_ZTC_20160607_oldest

code of robot bike

Dependencies:   SDFileSystem mbed

Fork of Robot_Bicycle by Chris LU

main.cpp@5:2290732b2782, 2016-07-06 (annotated)

Committer:
YCTung
Date:
Wed Jul 06 13:22:03 2016 +0000
Revision:
5:2290732b2782
Parent:
4:b0967990e390
Child:
6:bd469c945e41
be able to ride

Who changed what in which revision?

UserRevisionLine numberNew contents of line
YCTung 0:830ddddc129f 1 #include "mbed.h"
YCTung 0:830ddddc129f 2
YCTung 0:830ddddc129f 3 #include "RobotServo.h"
YCTung 0:830ddddc129f 4 #include "SensorFusion.h"
YCTung 0:830ddddc129f 5 #include "SPI_9dSensor.h"
YCTung 0:830ddddc129f 6 #include "RobotBicycleConst.h"
YCTung 3:197b748a397a 7 #include "ZTC.h"
YCTung 3:197b748a397a 8 //************************************************//
YCTung 3:197b748a397a 9 //**************** Wiring Map ********************//
YCTung 3:197b748a397a 10 //CK3 PC_10 | PC_11 MI3||re PC_9 | PC_8 rw
YCTung 3:197b748a397a 11 //MO3 PC_12 | PD_2 || |-----------------|
YCTung 3:197b748a397a 12 // 3.3V | E5V ||rs |PB_8 D15 | PC_6 |TX6
YCTung 3:197b748a397a 13 // BOOT0 | GND ||ra |PB_9 D14 | PC_5 |
YCTung 3:197b748a397a 14 // |---------------| || |AVDD | U5V |
YCTung 3:197b748a397a 15 // |NC | NC | || |GND | NC |
YCTung 3:197b748a397a 16 // |NC | IOREF | ||rk |PA_5 D13 | PA_12|RX6
YCTung 3:197b748a397a 17 // |PA_13 | NRST | || |PA_6 D12 | PA_11| rl
YCTung 3:197b748a397a 18 // |PA_14 | 3.3V | || |PA_7 D11 | PB_12|
YCTung 3:197b748a397a 19 // |PA_15 | 5.0V | ||rb |PB_6 D10 | NC |
YCTung 3:197b748a397a 20 // |GND | GND | || |PC_7 D9 | GND |
YCTung 3:197b748a397a 21 // |PB_7 | GND | ||lb |PA_9 D8 | PB_2 |CSG
YCTung 3:197b748a397a 22 // |PC_13 | VIN | || |-----------------|
YCTung 3:197b748a397a 23 // |---------------| || |-----------------|
YCTung 3:197b748a397a 24 // PC_14 | NC ||ll |PA_8 D7 | PB_1 |CSX
YCTung 3:197b748a397a 25 // |---------------| ||lk |PB_10 D6 | PB_15|MO2
YCTung 3:197b748a397a 26 // |PC_15 | PA_0 A0| ||la |PB_4 D5 | PB_14|MI2
YCTung 3:197b748a397a 27 // |PH_0 | PA_1 A1| ||ls |PB_5 D4 | PB_13|CK2
YCTung 3:197b748a397a 28 // |PH_1 | PA_4 A2| ||le |PB_3 D3 | AGND |
YCTung 3:197b748a397a 29 // |VBAT | PB_0 A3| ||lw |PA_10 D2 | PC_4 |
YCTung 4:b0967990e390 30 //out|PC_2 | PC_1 A4| ||TX2|PA_2 D1 | NC |
YCTung 4:b0967990e390 31 //out|PC_3 | PC_0 A5|IR ||RX2|PA_3 D0 | NC |
YCTung 3:197b748a397a 32 // |---------------| || |-----------------|
YCTung 3:197b748a397a 33 //************************************************//
YCTung 0:830ddddc129f 34
YCTung 3:197b748a397a 35 #define IR_FILT_CONST 1.0f * 2.0f * 3.14159f / 250.0f
YCTung 3:197b748a397a 36
YCTung 3:197b748a397a 37 #define CNT2ZTCm 250
YCTung 3:197b748a397a 38 #define CNT2ZTCh 5
YCTung 3:197b748a397a 39 #define CNT2STRT 1250
YCTung 3:197b748a397a 40
YCTung 0:830ddddc129f 41 AnalogIn analog_value(A5);//When using the ADC module, those pins connected to AnalogIn can't be used to output.
YCTung 0:830ddddc129f 42
YCTung 0:830ddddc129f 43 Ticker timer1;
YCTung 0:830ddddc129f 44 Ticker timer2;
YCTung 2:ec30613f2b2b 45 Timeout timeout;
YCTung 0:830ddddc129f 46
YCTung 4:b0967990e390 47 DigitalOut pin_pc2(PC_2);
YCTung 4:b0967990e390 48 DigitalOut pin_pc3(PC_3);
YCTung 4:b0967990e390 49
YCTung 2:ec30613f2b2b 50 Serial BT(PC_6, PA_12);
YCTung 2:ec30613f2b2b 51
YCTung 4:b0967990e390 52 SPI SD_card(PC_12, PC_11, PC_10);
YCTung 0:830ddddc129f 53
YCTung 3:197b748a397a 54 //**** Variables from Arduino ****//
YCTung 3:197b748a397a 55 int counter = 0;
YCTung 3:197b748a397a 56 int i = 0;
YCTung 3:197b748a397a 57 int8_t c = FIRST_POS;
YCTung 3:197b748a397a 58 bool s = 0;
YCTung 3:197b748a397a 59 int fusion_init = 0;
YCTung 3:197b748a397a 60 int state_count = 0;
YCTung 3:197b748a397a 61 uint8_t state_change = 0;
YCTung 3:197b748a397a 62 uint16_t brake_count = 0;
YCTung 3:197b748a397a 63 float V_meas = 0;
YCTung 3:197b748a397a 64 float L_inver = 0;
YCTung 3:197b748a397a 65 bool toggle = 0;
YCTung 3:197b748a397a 66 //********************************//
YCTung 3:197b748a397a 67
YCTung 3:197b748a397a 68 //***** Variables from RasPi *****//
YCTung 3:197b748a397a 69 char BTCharR = 0;
YCTung 3:197b748a397a 70
YCTung 3:197b748a397a 71 float Vx = 0.0;
YCTung 3:197b748a397a 72 float Vx_old = 0.0;
YCTung 3:197b748a397a 73
YCTung 3:197b748a397a 74 unsigned char pedal_state = 0;
YCTung 3:197b748a397a 75 unsigned char sys_state = S_S;
YCTung 3:197b748a397a 76 unsigned int count2ztc_h = 0;
YCTung 3:197b748a397a 77 unsigned int count2ztc_m = 0;
YCTung 3:197b748a397a 78 unsigned int count2straight = 0;
YCTung 3:197b748a397a 79 unsigned int count_isr = 0;
YCTung 3:197b748a397a 80 float T_total = 0.0;
YCTung 3:197b748a397a 81 //********************************//
YCTung 3:197b748a397a 82
YCTung 2:ec30613f2b2b 83 bool resetting = 0;
YCTung 0:830ddddc129f 84
YCTung 0:830ddddc129f 85 void timer1_interrupt(void);
YCTung 0:830ddddc129f 86 void timer2_interrupt(void);
YCTung 2:ec30613f2b2b 87 void reset_offset(void);
YCTung 5:2290732b2782 88 void ready_to_go(void);
YCTung 2:ec30613f2b2b 89 void attimeout(void);
YCTung 0:830ddddc129f 90
YCTung 0:830ddddc129f 91 int main() {
YCTung 4:b0967990e390 92 pin_pc2 = pin_pc3 = 0;
YCTung 4:b0967990e390 93
YCTung 0:830ddddc129f 94 setupServo();
YCTung 4:b0967990e390 95 pc.baud(115200);
YCTung 4:b0967990e390 96 setup_spi_sensor();
YCTung 2:ec30613f2b2b 97 init_Sensors();
YCTung 5:2290732b2782 98
YCTung 2:ec30613f2b2b 99 BT.baud(115200);
YCTung 2:ec30613f2b2b 100
YCTung 2:ec30613f2b2b 101 reset_offset();
YCTung 0:830ddddc129f 102
YCTung 0:830ddddc129f 103 timer1.attach_us(&timer1_interrupt, 4000);//4.0ms interrupt period (250 Hz)
YCTung 0:830ddddc129f 104 timer2.attach_us(&timer2_interrupt, 4098);//4.098ms interrupt period (244 Hz)
YCTung 0:830ddddc129f 105
YCTung 4:b0967990e390 106 SD_card.format(8, 3);
YCTung 4:b0967990e390 107 // fprintf_data(0);
YCTung 2:ec30613f2b2b 108 pc.printf("System ready.\r\n");
YCTung 2:ec30613f2b2b 109
YCTung 3:197b748a397a 110 reset_pos();
YCTung 3:197b748a397a 111 lookuptable_steering(HANDLE_START);
YCTung 4:b0967990e390 112 // lookuptable_steering(0);
YCTung 0:830ddddc129f 113
YCTung 3:197b748a397a 114 while(!interrupt)
YCTung 3:197b748a397a 115 {
YCTung 3:197b748a397a 116 if(BT.readable())
YCTung 3:197b748a397a 117 {
YCTung 3:197b748a397a 118 BTCharR = BT.getc();
YCTung 3:197b748a397a 119 switch(BTCharR)
YCTung 3:197b748a397a 120 {
YCTung 4:b0967990e390 121 case 's': reset_offset();
YCTung 5:2290732b2782 122 ready_to_go();
YCTung 5:2290732b2782 123 pc.printf("go\r\n");
YCTung 3:197b748a397a 124 pedal_state = 1; sys_state = L_PD; gamma_ref = 0.0;
YCTung 3:197b748a397a 125 s = 1; break; ///start
YCTung 5:2290732b2782 126 case 'a': pc.printf("stop\r\n");
YCTung 5:2290732b2782 127 pedal_state = 0; sys_state = S_S; Vx = 0.0; Vx_old = 0.0; gamma_ref = 0.0; count2ztc_h = 0; count2ztc_m = 0;
YCTung 3:197b748a397a 128 s = 0; state_count = 0; break; ///stop
YCTung 3:197b748a397a 129 // case 'm': pedal_state = 2; sys_state = M_PD; gamma_ref = 0.0; if(count2ztc_m >= CNT2ZTCm){sys_state = M_ZTC;} break;
YCTung 3:197b748a397a 130 // case 'h': pedal_state = 3; sys_state = H_PD; gamma_ref = 0.0; if(count2ztc_h >= CNT2ZTCh){sys_state = H_ZTC;} break;
YCTung 3:197b748a397a 131 case 'l': gamma_ref = DLT_GAMMA_REF; roll_ref = 0.0; count2straight = 0; break; ///left
YCTung 3:197b748a397a 132 case 'r': gamma_ref = -DLT_GAMMA_REF; roll_ref = 0.0; count2straight = 0; break; ///right
YCTung 3:197b748a397a 133 case 'f': gamma_ref = 0.0; roll_ref = 0.0; break; ///forward
YCTung 3:197b748a397a 134 case 'c': gamma_ref = 0.0; reset_offset(); break; ///clear
YCTung 3:197b748a397a 135 default: break;
YCTung 3:197b748a397a 136 }
YCTung 3:197b748a397a 137 BTCharR = 0;
YCTung 3:197b748a397a 138 }
YCTung 3:197b748a397a 139
YCTung 4:b0967990e390 140 sensorG_read_3axis();
YCTung 4:b0967990e390 141 sensorX_read_3axis();
YCTung 4:b0967990e390 142
YCTung 4:b0967990e390 143 get_9axis_scale();
YCTung 4:b0967990e390 144 get_9axis_data(pedal_state);
YCTung 4:b0967990e390 145
YCTung 3:197b748a397a 146 L_inver = 0.0063f * V_meas - 0.005769f;
YCTung 0:830ddddc129f 147 }
YCTung 0:830ddddc129f 148 }
YCTung 0:830ddddc129f 149
YCTung 0:830ddddc129f 150 void timer1_interrupt(void)
YCTung 0:830ddddc129f 151 {
YCTung 4:b0967990e390 152 // pin_pc2 = !pin_pc2;
YCTung 3:197b748a397a 153 V_meas = (analog_value.read() * 3.3f) * IR_FILT_CONST + V_meas * (1.0f - IR_FILT_CONST);
YCTung 4:b0967990e390 154
YCTung 4:b0967990e390 155 // 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));
YCTung 4:b0967990e390 156
YCTung 4:b0967990e390 157 roll_fusion(axm,aym,azm,u3,u1,1);
YCTung 4:b0967990e390 158 x1_hat_estimat(axm,aym,azm,u3,u2,Alpha);
YCTung 4:b0967990e390 159 m_x1_hat(mx,my,mz,u3,u2,Alpha);
YCTung 4:b0967990e390 160 m_x2_hat(mx,my,mz,u3,u1,Alpha);
YCTung 4:b0967990e390 161
YCTung 4:b0967990e390 162 if(cosroll != 0)
YCTung 4:b0967990e390 163 {
YCTung 4:b0967990e390 164 droll_angle = lpf(u1, droll_angle_old, 62.8);
YCTung 4:b0967990e390 165 droll_angle_old = droll_angle;
YCTung 4:b0967990e390 166 dyaw_angle = lpf(u3/cosroll, dyaw_angle_old, 3.1416);
YCTung 4:b0967990e390 167 dyaw_angle_old = dyaw_angle;
YCTung 4:b0967990e390 168 // m_yaw_fusion();
YCTung 4:b0967990e390 169
YCTung 4:b0967990e390 170 ///cut off
YCTung 4:b0967990e390 171 if(m_sinyaw <= -1.0f)m_sinyaw = -1.0f;
YCTung 4:b0967990e390 172 else if(m_sinyaw >= 1.0f)m_sinyaw = 1.0f;
YCTung 4:b0967990e390 173 else ;
YCTung 4:b0967990e390 174 if(m_cosyaw <= -1.0f)m_cosyaw = -1.0f;
YCTung 4:b0967990e390 175 else if(m_cosyaw >= 1.0f)m_cosyaw = 1.0f;
YCTung 4:b0967990e390 176 else ;
YCTung 4:b0967990e390 177
YCTung 4:b0967990e390 178 ///Controller
YCTung 4:b0967990e390 179 if(sys_state == L_PD)
YCTung 4:b0967990e390 180 {
YCTung 4:b0967990e390 181 calc_PD(K_l, 0.0);
YCTung 4:b0967990e390 182 calc_Gamma(u,15,b_h);
YCTung 4:b0967990e390 183 gamma_rad = gamma_rad + L_PD_OFFSET;
YCTung 4:b0967990e390 184
YCTung 4:b0967990e390 185 dphi_hat = 0.0;
YCTung 4:b0967990e390 186 phi_hat = 0.0;
YCTung 4:b0967990e390 187 yaw_angle = yaw_angle_old + dyaw_angle * sample_time;
YCTung 4:b0967990e390 188 yaw_angle_old = yaw_angle;
YCTung 4:b0967990e390 189 }
YCTung 4:b0967990e390 190 else if(sys_state == M_PD)
YCTung 4:b0967990e390 191 {
YCTung 4:b0967990e390 192 calc_PD(K_m, 0.0);
YCTung 4:b0967990e390 193 calc_Gamma(u,15,b_h);
YCTung 4:b0967990e390 194
YCTung 4:b0967990e390 195 dphi_hat = 0.0;
YCTung 4:b0967990e390 196 phi_hat = 0.0;
YCTung 4:b0967990e390 197 yaw_angle = yaw_angle_old + dyaw_angle * sample_time;
YCTung 4:b0967990e390 198 yaw_angle_old = yaw_angle;
YCTung 4:b0967990e390 199
YCTung 4:b0967990e390 200 count2ztc_m++;
YCTung 4:b0967990e390 201 if(count2ztc_m >= CNT2ZTCm){sys_state = M_ZTC;}
YCTung 4:b0967990e390 202 }
YCTung 4:b0967990e390 203 else if(sys_state == M_ZTC)
YCTung 4:b0967990e390 204 {
YCTung 4:b0967990e390 205 if(count2straight < CNT2STRT){count2straight++;}
YCTung 4:b0967990e390 206 else{gamma_ref = 0.0; roll_ref = 0.0;}
YCTung 4:b0967990e390 207 calc_PD(K_m, phi_hat);
YCTung 4:b0967990e390 208 calc_Phi(K_mphi);
YCTung 4:b0967990e390 209 calc_Gamma(u,15,b_h);
YCTung 4:b0967990e390 210
YCTung 4:b0967990e390 211 yaw_angle = yaw_angle_old + dyaw_angle * sample_time;
YCTung 4:b0967990e390 212 yaw_angle_old = yaw_angle;
YCTung 4:b0967990e390 213 }
YCTung 4:b0967990e390 214 else if(sys_state == H_PD)
YCTung 4:b0967990e390 215 {
YCTung 4:b0967990e390 216 calc_PD(K_h,0.0);
YCTung 4:b0967990e390 217 calc_Gamma(u,15,b_h);
YCTung 4:b0967990e390 218
YCTung 4:b0967990e390 219 dphi_hat = 0.0;
YCTung 4:b0967990e390 220 phi_hat = 0.0;
YCTung 4:b0967990e390 221 yaw_angle = yaw_angle_old + dyaw_angle * sample_time;
YCTung 4:b0967990e390 222 yaw_angle_old = yaw_angle;
YCTung 4:b0967990e390 223
YCTung 4:b0967990e390 224 // count2ztc_h++;
YCTung 4:b0967990e390 225 if(count2ztc_h >= CNT2ZTCh){sys_state = H_ZTC;}
YCTung 4:b0967990e390 226 }
YCTung 4:b0967990e390 227 else if(sys_state == H_ZTC)
YCTung 4:b0967990e390 228 {
YCTung 4:b0967990e390 229 if(count2straight < CNT2STRT){count2straight++;}
YCTung 4:b0967990e390 230 else{gamma_ref = 0.0; roll_ref = 0.0;}
YCTung 4:b0967990e390 231 calc_PD(K_h, phi_hat);
YCTung 4:b0967990e390 232 calc_Phi(K_hphi);
YCTung 4:b0967990e390 233 calc_Gamma(u,15,b_h);
YCTung 4:b0967990e390 234
YCTung 4:b0967990e390 235 yaw_angle = yaw_angle_old + dyaw_angle * sample_time;
YCTung 4:b0967990e390 236 yaw_angle_old = yaw_angle;
YCTung 4:b0967990e390 237 }
YCTung 4:b0967990e390 238 else
YCTung 4:b0967990e390 239 {
YCTung 4:b0967990e390 240 u = 0.0;
YCTung 5:2290732b2782 241 // calc_PD(K_l, 0.0);
YCTung 4:b0967990e390 242 calc_Gamma(u,15,b_h);
YCTung 4:b0967990e390 243 dphi_hat = 0.0;
YCTung 4:b0967990e390 244 phi_hat = 0.0;
YCTung 4:b0967990e390 245 yaw_angle = 0.0;
YCTung 4:b0967990e390 246 yaw_angle_old = 0.0;
YCTung 4:b0967990e390 247 }
YCTung 4:b0967990e390 248
YCTung 4:b0967990e390 249 ///Anti-Windup
YCTung 4:b0967990e390 250 anti_wdup();
YCTung 4:b0967990e390 251
YCTung 4:b0967990e390 252 // calc_Gamma(u,15,b_h);
YCTung 4:b0967990e390 253 if(gamma_rad > 0.349f)gamma_rad = 0.349f;
YCTung 4:b0967990e390 254 else if(gamma_rad < -0.349f)gamma_rad = -0.349f;
YCTung 4:b0967990e390 255 else ;
YCTung 4:b0967990e390 256 // 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);
YCTung 4:b0967990e390 257
YCTung 4:b0967990e390 258 ///Show results or Send datas
YCTung 4:b0967990e390 259 gamma_degree = (short int)(gamma_rad*114.59f);
YCTung 4:b0967990e390 260 if(gamma_degree > 40)gamma_degree = 40;
YCTung 4:b0967990e390 261 else if(gamma_degree < -40)gamma_degree = -40;
YCTung 4:b0967990e390 262 else ;
YCTung 4:b0967990e390 263
YCTung 4:b0967990e390 264 ///Steering
YCTung 4:b0967990e390 265 if(s == 1)
YCTung 4:b0967990e390 266 {
YCTung 4:b0967990e390 267 if(state_count > COUN2_HANDLE_START)
YCTung 4:b0967990e390 268 {
YCTung 4:b0967990e390 269 lookuptable_steering( gamma_degree );
YCTung 4:b0967990e390 270 }
YCTung 4:b0967990e390 271 else
YCTung 4:b0967990e390 272 {
YCTung 4:b0967990e390 273 lookuptable_steering( gamma_degree + HANDLE_START_BAL);
YCTung 4:b0967990e390 274 }
YCTung 4:b0967990e390 275 }
YCTung 4:b0967990e390 276 else
YCTung 4:b0967990e390 277 {
YCTung 4:b0967990e390 278 if(c == FIRST_POS)
YCTung 4:b0967990e390 279 {
YCTung 4:b0967990e390 280 if(state_change > 0)
YCTung 4:b0967990e390 281 {
YCTung 4:b0967990e390 282 lookuptable_steering(HANDLE_STOP);
YCTung 4:b0967990e390 283 }
YCTung 4:b0967990e390 284 else
YCTung 4:b0967990e390 285 {
YCTung 5:2290732b2782 286 if(!resetting)
YCTung 5:2290732b2782 287 {
YCTung 5:2290732b2782 288 lookuptable_steering(HANDLE_START);
YCTung 5:2290732b2782 289 // lookuptable_steering(HANDLE_STRAIGHT);
YCTung 5:2290732b2782 290 // lookuptable_steering( gamma_degree );
YCTung 5:2290732b2782 291 }
YCTung 5:2290732b2782 292 else ;
YCTung 4:b0967990e390 293 }
YCTung 4:b0967990e390 294 }
YCTung 4:b0967990e390 295 else lookuptable_steering( gamma_degree + HANDLE_START_BAL);
YCTung 4:b0967990e390 296 }
YCTung 4:b0967990e390 297
YCTung 4:b0967990e390 298 ///print file
YCTung 4:b0967990e390 299 if(sys_state >= 1)
YCTung 4:b0967990e390 300 {
YCTung 4:b0967990e390 301 count_isr++;
YCTung 4:b0967990e390 302 T_total = (float)sample_time*count_isr;
YCTung 4:b0967990e390 303
YCTung 4:b0967990e390 304 ///Integrate Ax to get Vx
YCTung 4:b0967990e390 305 Vx = Vx_old - axm_f*sample_time;
YCTung 4:b0967990e390 306 Vx_old = Vx;
YCTung 4:b0967990e390 307 // if(Vx >= v_low && sys_state < M_PD){pedal_state = 2; sys_state = M_PD; gamma_ref = 0.0;}
YCTung 4:b0967990e390 308
YCTung 4:b0967990e390 309 // fprintf_data(1);
YCTung 4:b0967990e390 310 }
YCTung 4:b0967990e390 311 }
YCTung 4:b0967990e390 312 else
YCTung 4:b0967990e390 313 {
YCTung 4:b0967990e390 314 roll_fusion(0,0,0,0,0,1);
YCTung 4:b0967990e390 315 gamma_degree = 0;
YCTung 4:b0967990e390 316 }
YCTung 0:830ddddc129f 317 }
YCTung 0:830ddddc129f 318
YCTung 0:830ddddc129f 319 void timer2_interrupt(void)
YCTung 0:830ddddc129f 320 {
YCTung 4:b0967990e390 321 // pin_pc3 = !pin_pc3;
YCTung 3:197b748a397a 322 if(s == 1) // bluetooth start
YCTung 3:197b748a397a 323 {
YCTung 3:197b748a397a 324 i ++;
YCTung 3:197b748a397a 325 if(i == Z_PEDAL_DIVIDER && state_change == 0) //start
YCTung 3:197b748a397a 326 {
YCTung 3:197b748a397a 327 i = 0;
YCTung 3:197b748a397a 328 lookuptable_pedaling(c);
YCTung 3:197b748a397a 329 c++;
YCTung 3:197b748a397a 330 if(c == (FIRST_POS + TOT_STOP_POS)){c = FIRST_POS + TOT_STOP_POS + TOT_STOP_POS - 1;}
YCTung 3:197b748a397a 331 else if(c >= TOT_FOOT_POS){c = 0;}
YCTung 3:197b748a397a 332 state_count++;
YCTung 3:197b748a397a 333 }
YCTung 3:197b748a397a 334 else if(i == L_PEDAL_DIVIDER && state_change == 1) //low_v
YCTung 3:197b748a397a 335 {
YCTung 3:197b748a397a 336 i = 0;
YCTung 3:197b748a397a 337 lookuptable_pedaling(c);
YCTung 3:197b748a397a 338 c++;
YCTung 3:197b748a397a 339 if(c == FIRST_POS){c = FIRST_POS + TOT_STOP_POS;}
YCTung 3:197b748a397a 340 else if(c >= TOT_FOOT_POS){c = 0;}
YCTung 3:197b748a397a 341 state_count++;
YCTung 3:197b748a397a 342 }
YCTung 3:197b748a397a 343 else if(i == M_PEDAL_DIVIDER && state_change == 2) //mid_v
YCTung 3:197b748a397a 344 {
YCTung 3:197b748a397a 345 i = 0;
YCTung 3:197b748a397a 346 lookuptable_pedaling(c);
YCTung 3:197b748a397a 347 c++;
YCTung 3:197b748a397a 348 if(c == FIRST_POS){c = FIRST_POS + TOT_STOP_POS;}
YCTung 3:197b748a397a 349 else if(c >= TOT_FOOT_POS){c = 0;}
YCTung 3:197b748a397a 350 state_count++;
YCTung 3:197b748a397a 351 }
YCTung 3:197b748a397a 352 else if(i == M_PEDAL_DIVIDER && state_change == 3) //high_v
YCTung 3:197b748a397a 353 {
YCTung 3:197b748a397a 354 i = 0;
YCTung 3:197b748a397a 355 lookuptable_pedaling(c);
YCTung 3:197b748a397a 356 c++;
YCTung 3:197b748a397a 357 if(c == FIRST_POS){c = FIRST_POS + TOT_STOP_POS;}
YCTung 3:197b748a397a 358 else if(c >= TOT_FOOT_POS){c = 0;}
YCTung 3:197b748a397a 359 }
YCTung 3:197b748a397a 360
YCTung 3:197b748a397a 361 if(state_count == COUN2_LOW_V)
YCTung 3:197b748a397a 362 {
YCTung 3:197b748a397a 363 state_change = 1;
YCTung 3:197b748a397a 364 }
YCTung 3:197b748a397a 365 else if(state_count == COUN2_MID_V)
YCTung 3:197b748a397a 366 {
YCTung 3:197b748a397a 367 state_change = 2;
YCTung 3:197b748a397a 368 }
YCTung 3:197b748a397a 369 else if(state_count == COUN2_HIGH_V)
YCTung 3:197b748a397a 370 {
YCTung 3:197b748a397a 371 if(state_change == 2)
YCTung 3:197b748a397a 372 {
YCTung 5:2290732b2782 373 pc.printf("m\r\n");
YCTung 5:2290732b2782 374 // BT.printf("m\r\n");
YCTung 3:197b748a397a 375 pedal_state = 2;
YCTung 3:197b748a397a 376 sys_state = M_PD;
YCTung 3:197b748a397a 377 gamma_ref = 0.0;
YCTung 3:197b748a397a 378 if(count2ztc_m >= CNT2ZTCm)
YCTung 3:197b748a397a 379 {
YCTung 3:197b748a397a 380 sys_state = M_ZTC;
YCTung 3:197b748a397a 381 }
YCTung 3:197b748a397a 382 }
YCTung 3:197b748a397a 383 state_change = 3;
YCTung 3:197b748a397a 384 }
YCTung 3:197b748a397a 385 }
YCTung 3:197b748a397a 386 else //s is 0
YCTung 3:197b748a397a 387 {
YCTung 3:197b748a397a 388 if(c == FIRST_POS)
YCTung 3:197b748a397a 389 {
YCTung 3:197b748a397a 390 if(state_change > 0)
YCTung 3:197b748a397a 391 {
YCTung 3:197b748a397a 392 stop_pos();
YCTung 3:197b748a397a 393 brake_count++;
YCTung 3:197b748a397a 394 if(brake_count >= 3*244)
YCTung 3:197b748a397a 395 {
YCTung 3:197b748a397a 396 state_change = 0;
YCTung 3:197b748a397a 397 brake_count = 0;
YCTung 3:197b748a397a 398 }
YCTung 3:197b748a397a 399 }
YCTung 3:197b748a397a 400 else
YCTung 3:197b748a397a 401 {
YCTung 5:2290732b2782 402 reset_pos();
YCTung 3:197b748a397a 403 }
YCTung 3:197b748a397a 404 }
YCTung 3:197b748a397a 405 else
YCTung 3:197b748a397a 406 {
YCTung 3:197b748a397a 407 i++;
YCTung 3:197b748a397a 408 if(i == M_PEDAL_DIVIDER)
YCTung 3:197b748a397a 409 {
YCTung 3:197b748a397a 410 i = 0;
YCTung 3:197b748a397a 411 lookuptable_pedaling(c);
YCTung 3:197b748a397a 412 c++;
YCTung 3:197b748a397a 413 if(c > FIRST_POS && c < (FIRST_POS + TOT_STOP_POS)){c = FIRST_POS;}
YCTung 3:197b748a397a 414 else if(c >= TOT_FOOT_POS){c = 0;}
YCTung 3:197b748a397a 415 }
YCTung 3:197b748a397a 416 }
YCTung 3:197b748a397a 417 }
YCTung 2:ec30613f2b2b 418 }
YCTung 2:ec30613f2b2b 419
YCTung 2:ec30613f2b2b 420 void reset_offset(void)
YCTung 2:ec30613f2b2b 421 {
YCTung 2:ec30613f2b2b 422 pc.printf("Reseting gyro and accel-X offset...\r\n");
YCTung 2:ec30613f2b2b 423 resetting = 1;
YCTung 2:ec30613f2b2b 424 timeout.attach(&attimeout, 2.0f);
YCTung 2:ec30613f2b2b 425 while(resetting)
YCTung 2:ec30613f2b2b 426 {
YCTung 2:ec30613f2b2b 427 reset_gyro_offset();
YCTung 2:ec30613f2b2b 428 reset_acceX_offset();
YCTung 2:ec30613f2b2b 429 }
YCTung 2:ec30613f2b2b 430 timeout.detach();
YCTung 2:ec30613f2b2b 431 pc.printf("Done reseting.\r\n");
YCTung 2:ec30613f2b2b 432 // pc.printf("%d\r\n", Acce_axis_zero[0]);
YCTung 2:ec30613f2b2b 433 }
YCTung 2:ec30613f2b2b 434
YCTung 5:2290732b2782 435 void ready_to_go(void)
YCTung 5:2290732b2782 436 {
YCTung 5:2290732b2782 437 resetting = 1;
YCTung 5:2290732b2782 438 lookuptable_pedaling(FIRST_POS);
YCTung 5:2290732b2782 439 lookuptable_steering(HANDLE_STRAIGHT);
YCTung 5:2290732b2782 440 timeout.attach(&attimeout, 1.0f);
YCTung 5:2290732b2782 441 while(resetting)
YCTung 5:2290732b2782 442 {
YCTung 5:2290732b2782 443 wait_ms(100);
YCTung 5:2290732b2782 444 }
YCTung 5:2290732b2782 445 timeout.detach();
YCTung 5:2290732b2782 446 }
YCTung 5:2290732b2782 447
YCTung 2:ec30613f2b2b 448 void attimeout(void)
YCTung 2:ec30613f2b2b 449 {
YCTung 2:ec30613f2b2b 450 resetting = 0;
YCTung 5:2290732b2782 451 }
YCTung 5:2290732b2782 452 //void attimeout2(void)
YCTung 5:2290732b2782 453 //{
YCTung 5:2290732b2782 454 // preparing = 0;
YCTung 5:2290732b2782 455 //}