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@4:b0967990e390, 2016-07-06 (annotated)

Committer:
YCTung
Date:
Wed Jul 06 06:54:15 2016 +0000
Revision:
4:b0967990e390
Parent:
3:197b748a397a
Child:
5:2290732b2782
update program

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 2:ec30613f2b2b 88 void attimeout(void);
YCTung 0:830ddddc129f 89
YCTung 0:830ddddc129f 90 int main() {
YCTung 4:b0967990e390 91 pin_pc2 = pin_pc3 = 0;
YCTung 4:b0967990e390 92
YCTung 0:830ddddc129f 93 setupServo();
YCTung 4:b0967990e390 94 pc.baud(115200);
YCTung 4:b0967990e390 95 setup_spi_sensor();
YCTung 2:ec30613f2b2b 96 init_Sensors();
YCTung 2:ec30613f2b2b 97 BT.baud(115200);
YCTung 2:ec30613f2b2b 98
YCTung 2:ec30613f2b2b 99 reset_offset();
YCTung 0:830ddddc129f 100
YCTung 0:830ddddc129f 101 timer1.attach_us(&timer1_interrupt, 4000);//4.0ms interrupt period (250 Hz)
YCTung 0:830ddddc129f 102 timer2.attach_us(&timer2_interrupt, 4098);//4.098ms interrupt period (244 Hz)
YCTung 0:830ddddc129f 103
YCTung 4:b0967990e390 104 SD_card.format(8, 3);
YCTung 4:b0967990e390 105 // fprintf_data(0);
YCTung 2:ec30613f2b2b 106 pc.printf("System ready.\r\n");
YCTung 2:ec30613f2b2b 107
YCTung 3:197b748a397a 108 reset_pos();
YCTung 3:197b748a397a 109 lookuptable_steering(HANDLE_START);
YCTung 4:b0967990e390 110 // lookuptable_steering(0);
YCTung 0:830ddddc129f 111
YCTung 3:197b748a397a 112 while(!interrupt)
YCTung 3:197b748a397a 113 {
YCTung 3:197b748a397a 114 if(BT.readable())
YCTung 3:197b748a397a 115 {
YCTung 3:197b748a397a 116 BTCharR = BT.getc();
YCTung 3:197b748a397a 117 switch(BTCharR)
YCTung 3:197b748a397a 118 {
YCTung 4:b0967990e390 119 case 's': reset_offset();
YCTung 4:b0967990e390 120 lookuptable_pedaling(FIRST_POS);
YCTung 3:197b748a397a 121 lookuptable_steering(HANDLE_STRAIGHT);
YCTung 3:197b748a397a 122 wait(1.5f);
YCTung 3:197b748a397a 123 pedal_state = 1; sys_state = L_PD; gamma_ref = 0.0;
YCTung 3:197b748a397a 124 s = 1; break; ///start
YCTung 3:197b748a397a 125 case 'a': 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 126 s = 0; state_count = 0; break; ///stop
YCTung 3:197b748a397a 127 // 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 128 // 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 129 case 'l': gamma_ref = DLT_GAMMA_REF; roll_ref = 0.0; count2straight = 0; break; ///left
YCTung 3:197b748a397a 130 case 'r': gamma_ref = -DLT_GAMMA_REF; roll_ref = 0.0; count2straight = 0; break; ///right
YCTung 3:197b748a397a 131 case 'f': gamma_ref = 0.0; roll_ref = 0.0; break; ///forward
YCTung 3:197b748a397a 132 case 'c': gamma_ref = 0.0; reset_offset(); break; ///clear
YCTung 3:197b748a397a 133 default: break;
YCTung 3:197b748a397a 134 }
YCTung 3:197b748a397a 135 BTCharR = 0;
YCTung 3:197b748a397a 136 }
YCTung 3:197b748a397a 137
YCTung 4:b0967990e390 138 sensorG_read_3axis();
YCTung 4:b0967990e390 139 sensorX_read_3axis();
YCTung 4:b0967990e390 140
YCTung 4:b0967990e390 141 get_9axis_scale();
YCTung 4:b0967990e390 142 get_9axis_data(pedal_state);
YCTung 4:b0967990e390 143
YCTung 3:197b748a397a 144 L_inver = 0.0063f * V_meas - 0.005769f;
YCTung 0:830ddddc129f 145 }
YCTung 0:830ddddc129f 146 }
YCTung 0:830ddddc129f 147
YCTung 0:830ddddc129f 148 void timer1_interrupt(void)
YCTung 0:830ddddc129f 149 {
YCTung 4:b0967990e390 150 // pin_pc2 = !pin_pc2;
YCTung 3:197b748a397a 151 V_meas = (analog_value.read() * 3.3f) * IR_FILT_CONST + V_meas * (1.0f - IR_FILT_CONST);
YCTung 4:b0967990e390 152
YCTung 4:b0967990e390 153 // 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 154
YCTung 4:b0967990e390 155 roll_fusion(axm,aym,azm,u3,u1,1);
YCTung 4:b0967990e390 156 x1_hat_estimat(axm,aym,azm,u3,u2,Alpha);
YCTung 4:b0967990e390 157 m_x1_hat(mx,my,mz,u3,u2,Alpha);
YCTung 4:b0967990e390 158 m_x2_hat(mx,my,mz,u3,u1,Alpha);
YCTung 4:b0967990e390 159
YCTung 4:b0967990e390 160 if(cosroll != 0)
YCTung 4:b0967990e390 161 {
YCTung 4:b0967990e390 162 droll_angle = lpf(u1, droll_angle_old, 62.8);
YCTung 4:b0967990e390 163 droll_angle_old = droll_angle;
YCTung 4:b0967990e390 164 dyaw_angle = lpf(u3/cosroll, dyaw_angle_old, 3.1416);
YCTung 4:b0967990e390 165 dyaw_angle_old = dyaw_angle;
YCTung 4:b0967990e390 166 // m_yaw_fusion();
YCTung 4:b0967990e390 167
YCTung 4:b0967990e390 168 ///cut off
YCTung 4:b0967990e390 169 if(m_sinyaw <= -1.0f)m_sinyaw = -1.0f;
YCTung 4:b0967990e390 170 else if(m_sinyaw >= 1.0f)m_sinyaw = 1.0f;
YCTung 4:b0967990e390 171 else ;
YCTung 4:b0967990e390 172 if(m_cosyaw <= -1.0f)m_cosyaw = -1.0f;
YCTung 4:b0967990e390 173 else if(m_cosyaw >= 1.0f)m_cosyaw = 1.0f;
YCTung 4:b0967990e390 174 else ;
YCTung 4:b0967990e390 175
YCTung 4:b0967990e390 176 ///Controller
YCTung 4:b0967990e390 177 if(sys_state == L_PD)
YCTung 4:b0967990e390 178 {
YCTung 4:b0967990e390 179 calc_PD(K_l, 0.0);
YCTung 4:b0967990e390 180 calc_Gamma(u,15,b_h);
YCTung 4:b0967990e390 181 gamma_rad = gamma_rad + L_PD_OFFSET;
YCTung 4:b0967990e390 182
YCTung 4:b0967990e390 183 dphi_hat = 0.0;
YCTung 4:b0967990e390 184 phi_hat = 0.0;
YCTung 4:b0967990e390 185 yaw_angle = yaw_angle_old + dyaw_angle * sample_time;
YCTung 4:b0967990e390 186 yaw_angle_old = yaw_angle;
YCTung 4:b0967990e390 187 }
YCTung 4:b0967990e390 188 else if(sys_state == M_PD)
YCTung 4:b0967990e390 189 {
YCTung 4:b0967990e390 190 calc_PD(K_m, 0.0);
YCTung 4:b0967990e390 191 calc_Gamma(u,15,b_h);
YCTung 4:b0967990e390 192
YCTung 4:b0967990e390 193 dphi_hat = 0.0;
YCTung 4:b0967990e390 194 phi_hat = 0.0;
YCTung 4:b0967990e390 195 yaw_angle = yaw_angle_old + dyaw_angle * sample_time;
YCTung 4:b0967990e390 196 yaw_angle_old = yaw_angle;
YCTung 4:b0967990e390 197
YCTung 4:b0967990e390 198 count2ztc_m++;
YCTung 4:b0967990e390 199 if(count2ztc_m >= CNT2ZTCm){sys_state = M_ZTC;}
YCTung 4:b0967990e390 200 }
YCTung 4:b0967990e390 201 else if(sys_state == M_ZTC)
YCTung 4:b0967990e390 202 {
YCTung 4:b0967990e390 203 if(count2straight < CNT2STRT){count2straight++;}
YCTung 4:b0967990e390 204 else{gamma_ref = 0.0; roll_ref = 0.0;}
YCTung 4:b0967990e390 205 calc_PD(K_m, phi_hat);
YCTung 4:b0967990e390 206 calc_Phi(K_mphi);
YCTung 4:b0967990e390 207 calc_Gamma(u,15,b_h);
YCTung 4:b0967990e390 208
YCTung 4:b0967990e390 209 yaw_angle = yaw_angle_old + dyaw_angle * sample_time;
YCTung 4:b0967990e390 210 yaw_angle_old = yaw_angle;
YCTung 4:b0967990e390 211 }
YCTung 4:b0967990e390 212 else if(sys_state == H_PD)
YCTung 4:b0967990e390 213 {
YCTung 4:b0967990e390 214 calc_PD(K_h,0.0);
YCTung 4:b0967990e390 215 calc_Gamma(u,15,b_h);
YCTung 4:b0967990e390 216
YCTung 4:b0967990e390 217 dphi_hat = 0.0;
YCTung 4:b0967990e390 218 phi_hat = 0.0;
YCTung 4:b0967990e390 219 yaw_angle = yaw_angle_old + dyaw_angle * sample_time;
YCTung 4:b0967990e390 220 yaw_angle_old = yaw_angle;
YCTung 4:b0967990e390 221
YCTung 4:b0967990e390 222 // count2ztc_h++;
YCTung 4:b0967990e390 223 if(count2ztc_h >= CNT2ZTCh){sys_state = H_ZTC;}
YCTung 4:b0967990e390 224 }
YCTung 4:b0967990e390 225 else if(sys_state == H_ZTC)
YCTung 4:b0967990e390 226 {
YCTung 4:b0967990e390 227 if(count2straight < CNT2STRT){count2straight++;}
YCTung 4:b0967990e390 228 else{gamma_ref = 0.0; roll_ref = 0.0;}
YCTung 4:b0967990e390 229 calc_PD(K_h, phi_hat);
YCTung 4:b0967990e390 230 calc_Phi(K_hphi);
YCTung 4:b0967990e390 231 calc_Gamma(u,15,b_h);
YCTung 4:b0967990e390 232
YCTung 4:b0967990e390 233 yaw_angle = yaw_angle_old + dyaw_angle * sample_time;
YCTung 4:b0967990e390 234 yaw_angle_old = yaw_angle;
YCTung 4:b0967990e390 235 }
YCTung 4:b0967990e390 236 else
YCTung 4:b0967990e390 237 {
YCTung 4:b0967990e390 238 u = 0.0;
YCTung 4:b0967990e390 239 calc_Gamma(u,15,b_h);
YCTung 4:b0967990e390 240 dphi_hat = 0.0;
YCTung 4:b0967990e390 241 phi_hat = 0.0;
YCTung 4:b0967990e390 242 yaw_angle = 0.0;
YCTung 4:b0967990e390 243 yaw_angle_old = 0.0;
YCTung 4:b0967990e390 244 }
YCTung 4:b0967990e390 245
YCTung 4:b0967990e390 246 ///Anti-Windup
YCTung 4:b0967990e390 247 anti_wdup();
YCTung 4:b0967990e390 248
YCTung 4:b0967990e390 249 // calc_Gamma(u,15,b_h);
YCTung 4:b0967990e390 250 if(gamma_rad > 0.349f)gamma_rad = 0.349f;
YCTung 4:b0967990e390 251 else if(gamma_rad < -0.349f)gamma_rad = -0.349f;
YCTung 4:b0967990e390 252 else ;
YCTung 4:b0967990e390 253 // 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 254
YCTung 4:b0967990e390 255 ///Show results or Send datas
YCTung 4:b0967990e390 256 gamma_degree = (short int)(gamma_rad*114.59f);
YCTung 4:b0967990e390 257 if(gamma_degree > 40)gamma_degree = 40;
YCTung 4:b0967990e390 258 else if(gamma_degree < -40)gamma_degree = -40;
YCTung 4:b0967990e390 259 else ;
YCTung 4:b0967990e390 260
YCTung 4:b0967990e390 261 ///Steering
YCTung 4:b0967990e390 262 if(s == 1)
YCTung 4:b0967990e390 263 {
YCTung 4:b0967990e390 264 if(state_count > COUN2_HANDLE_START)
YCTung 4:b0967990e390 265 {
YCTung 4:b0967990e390 266 lookuptable_steering( gamma_degree );
YCTung 4:b0967990e390 267 }
YCTung 4:b0967990e390 268 else
YCTung 4:b0967990e390 269 {
YCTung 4:b0967990e390 270 lookuptable_steering( gamma_degree + HANDLE_START_BAL);
YCTung 4:b0967990e390 271 }
YCTung 4:b0967990e390 272 }
YCTung 4:b0967990e390 273 else
YCTung 4:b0967990e390 274 {
YCTung 4:b0967990e390 275 if(c == FIRST_POS)
YCTung 4:b0967990e390 276 {
YCTung 4:b0967990e390 277 if(state_change > 0)
YCTung 4:b0967990e390 278 {
YCTung 4:b0967990e390 279 lookuptable_steering(HANDLE_STOP);
YCTung 4:b0967990e390 280 }
YCTung 4:b0967990e390 281 else
YCTung 4:b0967990e390 282 {
YCTung 4:b0967990e390 283 lookuptable_steering(HANDLE_START);
YCTung 4:b0967990e390 284 // lookuptable_steering(HANDLE_STRAIGHT);
YCTung 4:b0967990e390 285 // lookuptable_steering( gamma_degree );
YCTung 4:b0967990e390 286 }
YCTung 4:b0967990e390 287 }
YCTung 4:b0967990e390 288 else lookuptable_steering( gamma_degree + HANDLE_START_BAL);
YCTung 4:b0967990e390 289 }
YCTung 4:b0967990e390 290
YCTung 4:b0967990e390 291 ///print file
YCTung 4:b0967990e390 292 if(sys_state >= 1)
YCTung 4:b0967990e390 293 {
YCTung 4:b0967990e390 294 count_isr++;
YCTung 4:b0967990e390 295 T_total = (float)sample_time*count_isr;
YCTung 4:b0967990e390 296
YCTung 4:b0967990e390 297 ///Integrate Ax to get Vx
YCTung 4:b0967990e390 298 Vx = Vx_old - axm_f*sample_time;
YCTung 4:b0967990e390 299 Vx_old = Vx;
YCTung 4:b0967990e390 300 // if(Vx >= v_low && sys_state < M_PD){pedal_state = 2; sys_state = M_PD; gamma_ref = 0.0;}
YCTung 4:b0967990e390 301
YCTung 4:b0967990e390 302 // fprintf_data(1);
YCTung 4:b0967990e390 303 }
YCTung 4:b0967990e390 304 }
YCTung 4:b0967990e390 305 else
YCTung 4:b0967990e390 306 {
YCTung 4:b0967990e390 307 roll_fusion(0,0,0,0,0,1);
YCTung 4:b0967990e390 308 gamma_degree = 0;
YCTung 4:b0967990e390 309 }
YCTung 0:830ddddc129f 310 }
YCTung 0:830ddddc129f 311
YCTung 0:830ddddc129f 312 void timer2_interrupt(void)
YCTung 0:830ddddc129f 313 {
YCTung 4:b0967990e390 314 // pin_pc3 = !pin_pc3;
YCTung 3:197b748a397a 315 if(s == 1) // bluetooth start
YCTung 3:197b748a397a 316 {
YCTung 3:197b748a397a 317 i ++;
YCTung 3:197b748a397a 318 if(i == Z_PEDAL_DIVIDER && state_change == 0) //start
YCTung 3:197b748a397a 319 {
YCTung 3:197b748a397a 320 i = 0;
YCTung 3:197b748a397a 321 lookuptable_pedaling(c);
YCTung 3:197b748a397a 322 c++;
YCTung 3:197b748a397a 323 if(c == (FIRST_POS + TOT_STOP_POS)){c = FIRST_POS + TOT_STOP_POS + TOT_STOP_POS - 1;}
YCTung 3:197b748a397a 324 else if(c >= TOT_FOOT_POS){c = 0;}
YCTung 3:197b748a397a 325 state_count++;
YCTung 3:197b748a397a 326 }
YCTung 3:197b748a397a 327 else if(i == L_PEDAL_DIVIDER && state_change == 1) //low_v
YCTung 3:197b748a397a 328 {
YCTung 3:197b748a397a 329 i = 0;
YCTung 3:197b748a397a 330 lookuptable_pedaling(c);
YCTung 3:197b748a397a 331 c++;
YCTung 3:197b748a397a 332 if(c == FIRST_POS){c = FIRST_POS + TOT_STOP_POS;}
YCTung 3:197b748a397a 333 else if(c >= TOT_FOOT_POS){c = 0;}
YCTung 3:197b748a397a 334 state_count++;
YCTung 3:197b748a397a 335 }
YCTung 3:197b748a397a 336 else if(i == M_PEDAL_DIVIDER && state_change == 2) //mid_v
YCTung 3:197b748a397a 337 {
YCTung 3:197b748a397a 338 i = 0;
YCTung 3:197b748a397a 339 lookuptable_pedaling(c);
YCTung 3:197b748a397a 340 c++;
YCTung 3:197b748a397a 341 if(c == FIRST_POS){c = FIRST_POS + TOT_STOP_POS;}
YCTung 3:197b748a397a 342 else if(c >= TOT_FOOT_POS){c = 0;}
YCTung 3:197b748a397a 343 state_count++;
YCTung 3:197b748a397a 344 }
YCTung 3:197b748a397a 345 else if(i == M_PEDAL_DIVIDER && state_change == 3) //high_v
YCTung 3:197b748a397a 346 {
YCTung 3:197b748a397a 347 i = 0;
YCTung 3:197b748a397a 348 lookuptable_pedaling(c);
YCTung 3:197b748a397a 349 c++;
YCTung 3:197b748a397a 350 if(c == FIRST_POS){c = FIRST_POS + TOT_STOP_POS;}
YCTung 3:197b748a397a 351 else if(c >= TOT_FOOT_POS){c = 0;}
YCTung 3:197b748a397a 352 }
YCTung 3:197b748a397a 353
YCTung 3:197b748a397a 354 if(state_count == COUN2_LOW_V)
YCTung 3:197b748a397a 355 {
YCTung 3:197b748a397a 356 state_change = 1;
YCTung 3:197b748a397a 357 }
YCTung 3:197b748a397a 358 else if(state_count == COUN2_MID_V)
YCTung 3:197b748a397a 359 {
YCTung 3:197b748a397a 360 state_change = 2;
YCTung 3:197b748a397a 361 }
YCTung 3:197b748a397a 362 else if(state_count == COUN2_HIGH_V)
YCTung 3:197b748a397a 363 {
YCTung 3:197b748a397a 364 if(state_change == 2)
YCTung 3:197b748a397a 365 {
YCTung 3:197b748a397a 366 // pc.printf("\nm");
YCTung 3:197b748a397a 367 pedal_state = 2;
YCTung 3:197b748a397a 368 sys_state = M_PD;
YCTung 3:197b748a397a 369 gamma_ref = 0.0;
YCTung 3:197b748a397a 370 if(count2ztc_m >= CNT2ZTCm)
YCTung 3:197b748a397a 371 {
YCTung 3:197b748a397a 372 sys_state = M_ZTC;
YCTung 3:197b748a397a 373 }
YCTung 3:197b748a397a 374 }
YCTung 3:197b748a397a 375 state_change = 3;
YCTung 3:197b748a397a 376 }
YCTung 3:197b748a397a 377 }
YCTung 3:197b748a397a 378 else //s is 0
YCTung 3:197b748a397a 379 {
YCTung 3:197b748a397a 380 if(c == FIRST_POS)
YCTung 3:197b748a397a 381 {
YCTung 3:197b748a397a 382 if(state_change > 0)
YCTung 3:197b748a397a 383 {
YCTung 3:197b748a397a 384 stop_pos();
YCTung 3:197b748a397a 385 brake_count++;
YCTung 3:197b748a397a 386 if(brake_count >= 3*244)
YCTung 3:197b748a397a 387 {
YCTung 3:197b748a397a 388 state_change = 0;
YCTung 3:197b748a397a 389 brake_count = 0;
YCTung 3:197b748a397a 390 }
YCTung 3:197b748a397a 391 }
YCTung 3:197b748a397a 392 else
YCTung 3:197b748a397a 393 {
YCTung 4:b0967990e390 394 // reset_pos();
YCTung 3:197b748a397a 395 }
YCTung 3:197b748a397a 396 }
YCTung 3:197b748a397a 397 else
YCTung 3:197b748a397a 398 {
YCTung 3:197b748a397a 399 i++;
YCTung 3:197b748a397a 400 if(i == M_PEDAL_DIVIDER)
YCTung 3:197b748a397a 401 {
YCTung 3:197b748a397a 402 i = 0;
YCTung 3:197b748a397a 403 lookuptable_pedaling(c);
YCTung 3:197b748a397a 404 c++;
YCTung 3:197b748a397a 405 if(c > FIRST_POS && c < (FIRST_POS + TOT_STOP_POS)){c = FIRST_POS;}
YCTung 3:197b748a397a 406 else if(c >= TOT_FOOT_POS){c = 0;}
YCTung 3:197b748a397a 407 }
YCTung 3:197b748a397a 408 }
YCTung 3:197b748a397a 409 }
YCTung 2:ec30613f2b2b 410 }
YCTung 2:ec30613f2b2b 411
YCTung 2:ec30613f2b2b 412 void reset_offset(void)
YCTung 2:ec30613f2b2b 413 {
YCTung 2:ec30613f2b2b 414 pc.printf("Reseting gyro and accel-X offset...\r\n");
YCTung 2:ec30613f2b2b 415 resetting = 1;
YCTung 2:ec30613f2b2b 416 timeout.attach(&attimeout, 2.0f);
YCTung 2:ec30613f2b2b 417 while(resetting)
YCTung 2:ec30613f2b2b 418 {
YCTung 2:ec30613f2b2b 419 reset_gyro_offset();
YCTung 2:ec30613f2b2b 420 reset_acceX_offset();
YCTung 2:ec30613f2b2b 421 }
YCTung 2:ec30613f2b2b 422 timeout.detach();
YCTung 2:ec30613f2b2b 423 pc.printf("Done reseting.\r\n");
YCTung 2:ec30613f2b2b 424 // pc.printf("%d\r\n", Acce_axis_zero[0]);
YCTung 2:ec30613f2b2b 425 }
YCTung 2:ec30613f2b2b 426
YCTung 2:ec30613f2b2b 427 void attimeout(void)
YCTung 2:ec30613f2b2b 428 {
YCTung 2:ec30613f2b2b 429 resetting = 0;
YCTung 0:830ddddc129f 430 }