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@6:bd469c945e41, 2016-07-10 (annotated)

Committer:
YCTung
Date:
Sun Jul 10 06:23:02 2016 +0000
Revision:
6:bd469c945e41
Parent:
5:2290732b2782
Child:
7:b8413464d111
add SDFileSystem library

Who changed what in which revision?

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