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@10:3b952ea7fad4, 2016-07-18 (annotated)

Committer:
YCTung
Date:
Mon Jul 18 12:12:50 2016 +0000
Revision:
10:3b952ea7fad4
Parent:
9:159bad8bae03
Child:
12:60593247555d
modified some files. "interrupt" and "pc" declared in main.cpp

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 10:3b952ea7fad4 51 Serial pc(D1, D0);
YCTung 2:ec30613f2b2b 52 Serial BT(PC_6, PA_12);
YCTung 2:ec30613f2b2b 53
YCTung 6:bd469c945e41 54 SDFileSystem SD_card(PC_12, PC_11, PC_10, PD_2, "sd"); //mosi, miso, sck, cs, name
YCTung 0:830ddddc129f 55
YCTung 3:197b748a397a 56 //**** Variables from Arduino ****//
YCTung 3:197b748a397a 57 int counter = 0;
YCTung 3:197b748a397a 58 int i = 0;
YCTung 3:197b748a397a 59 int8_t c = FIRST_POS;
YCTung 3:197b748a397a 60 bool s = 0;
YCTung 3:197b748a397a 61 int fusion_init = 0;
YCTung 3:197b748a397a 62 int state_count = 0;
YCTung 3:197b748a397a 63 uint8_t state_change = 0;
YCTung 3:197b748a397a 64 uint16_t brake_count = 0;
YCTung 3:197b748a397a 65 float V_meas = 0;
YCTung 3:197b748a397a 66 float L_inver = 0;
YCTung 3:197b748a397a 67 bool toggle = 0;
YCTung 3:197b748a397a 68 //********************************//
YCTung 3:197b748a397a 69
YCTung 3:197b748a397a 70 //***** Variables from RasPi *****//
YCTung 10:3b952ea7fad4 71 bool interrupt = 0;
YCTung 3:197b748a397a 72 char BTCharR = 0;
YCTung 3:197b748a397a 73
YCTung 3:197b748a397a 74 float Vx = 0.0;
YCTung 3:197b748a397a 75 float Vx_old = 0.0;
YCTung 3:197b748a397a 76
YCTung 7:b8413464d111 77 unsigned char fall_down = 0;
YCTung 3:197b748a397a 78 unsigned char pedal_state = 0;
YCTung 3:197b748a397a 79 unsigned char sys_state = S_S;
YCTung 3:197b748a397a 80 unsigned int count2ztc_h = 0;
YCTung 3:197b748a397a 81 unsigned int count2ztc_m = 0;
YCTung 3:197b748a397a 82 unsigned int count2straight = 0;
YCTung 3:197b748a397a 83 unsigned int count_isr = 0;
YCTung 3:197b748a397a 84 float T_total = 0.0;
YCTung 7:b8413464d111 85
YCTung 7:b8413464d111 86 FILE *dataPtr;
YCTung 3:197b748a397a 87 //********************************//
YCTung 3:197b748a397a 88
YCTung 2:ec30613f2b2b 89 bool resetting = 0;
YCTung 0:830ddddc129f 90
YCTung 0:830ddddc129f 91 void timer1_interrupt(void);
YCTung 0:830ddddc129f 92 void timer2_interrupt(void);
YCTung 7:b8413464d111 93 void fprintf_data(unsigned char mode);
YCTung 2:ec30613f2b2b 94 void reset_offset(void);
YCTung 5:2290732b2782 95 void ready_to_go(void);
YCTung 2:ec30613f2b2b 96 void attimeout(void);
YCTung 0:830ddddc129f 97
YCTung 0:830ddddc129f 98 int main() {
YCTung 4:b0967990e390 99 pin_pc2 = pin_pc3 = 0;
YCTung 4:b0967990e390 100
YCTung 0:830ddddc129f 101 setupServo();
YCTung 4:b0967990e390 102 pc.baud(115200);
YCTung 10:3b952ea7fad4 103 BT.baud(115200);
YCTung 10:3b952ea7fad4 104 if(setup_spi_sensor())
YCTung 10:3b952ea7fad4 105 {
YCTung 10:3b952ea7fad4 106 interrupt = 0;
YCTung 10:3b952ea7fad4 107 pc.printf("SPI sensor ready.\r\n");
YCTung 10:3b952ea7fad4 108 }
YCTung 10:3b952ea7fad4 109 else interrupt = 1;
YCTung 2:ec30613f2b2b 110 init_Sensors();
YCTung 5:2290732b2782 111
YCTung 2:ec30613f2b2b 112 reset_offset();
YCTung 0:830ddddc129f 113
YCTung 0:830ddddc129f 114 timer1.attach_us(&timer1_interrupt, 4000);//4.0ms interrupt period (250 Hz)
YCTung 0:830ddddc129f 115 timer2.attach_us(&timer2_interrupt, 4098);//4.098ms interrupt period (244 Hz)
YCTung 0:830ddddc129f 116
YCTung 9:159bad8bae03 117 // mkdir("/sd/20160718", 0777);
YCTung 9:159bad8bae03 118 // dataPtr = fopen("/sd/20160718/data.dat", "w");
YCTung 9:159bad8bae03 119 dataPtr = fopen("/sd/data.dat", "w");
YCTung 7:b8413464d111 120 if(dataPtr == NULL) {
YCTung 7:b8413464d111 121 error("Could not open file for write\r\n");
YCTung 7:b8413464d111 122 interrupt = 1;
YCTung 7:b8413464d111 123 }
YCTung 7:b8413464d111 124 fprintf_data(0);
YCTung 2:ec30613f2b2b 125 pc.printf("System ready.\r\n");
YCTung 2:ec30613f2b2b 126
YCTung 3:197b748a397a 127 reset_pos();
YCTung 3:197b748a397a 128 lookuptable_steering(HANDLE_START);
YCTung 4:b0967990e390 129 // lookuptable_steering(0);
YCTung 0:830ddddc129f 130
YCTung 3:197b748a397a 131 while(!interrupt)
YCTung 3:197b748a397a 132 {
YCTung 3:197b748a397a 133 if(BT.readable())
YCTung 3:197b748a397a 134 {
YCTung 3:197b748a397a 135 BTCharR = BT.getc();
YCTung 3:197b748a397a 136 switch(BTCharR)
YCTung 3:197b748a397a 137 {
YCTung 6:bd469c945e41 138 case 's': //reset_offset();
YCTung 5:2290732b2782 139 ready_to_go();
YCTung 5:2290732b2782 140 pc.printf("go\r\n");
YCTung 3:197b748a397a 141 pedal_state = 1; sys_state = L_PD; gamma_ref = 0.0;
YCTung 3:197b748a397a 142 s = 1; break; ///start
YCTung 5:2290732b2782 143 case 'a': pc.printf("stop\r\n");
YCTung 5:2290732b2782 144 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 7:b8413464d111 145 s = 0; state_count = 0;
YCTung 9:159bad8bae03 146 break; ///stop
YCTung 3:197b748a397a 147 case 'l': gamma_ref = DLT_GAMMA_REF; roll_ref = 0.0; count2straight = 0; break; ///left
YCTung 3:197b748a397a 148 case 'r': gamma_ref = -DLT_GAMMA_REF; roll_ref = 0.0; count2straight = 0; break; ///right
YCTung 3:197b748a397a 149 case 'f': gamma_ref = 0.0; roll_ref = 0.0; break; ///forward
YCTung 9:159bad8bae03 150 case 'c': gamma_ref = 0.0; reset_offset(); break; ///calibrate
YCTung 9:159bad8bae03 151 case 'h': fclose(dataPtr); interrupt = 1; break; ///halt
YCTung 3:197b748a397a 152 default: break;
YCTung 3:197b748a397a 153 }
YCTung 3:197b748a397a 154 BTCharR = 0;
YCTung 3:197b748a397a 155 }
YCTung 3:197b748a397a 156
YCTung 4:b0967990e390 157 sensorG_read_3axis();
YCTung 4:b0967990e390 158 sensorX_read_3axis();
YCTung 4:b0967990e390 159
YCTung 4:b0967990e390 160 get_9axis_scale();
YCTung 4:b0967990e390 161 get_9axis_data(pedal_state);
YCTung 4:b0967990e390 162
YCTung 3:197b748a397a 163 L_inver = 0.0063f * V_meas - 0.005769f;
YCTung 0:830ddddc129f 164 }
YCTung 0:830ddddc129f 165 }
YCTung 0:830ddddc129f 166
YCTung 0:830ddddc129f 167 void timer1_interrupt(void)
YCTung 0:830ddddc129f 168 {
YCTung 4:b0967990e390 169 // pin_pc2 = !pin_pc2;
YCTung 3:197b748a397a 170 V_meas = (analog_value.read() * 3.3f) * IR_FILT_CONST + V_meas * (1.0f - IR_FILT_CONST);
YCTung 4:b0967990e390 171
YCTung 4:b0967990e390 172 // 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 173
YCTung 4:b0967990e390 174 roll_fusion(axm,aym,azm,u3,u1,1);
YCTung 4:b0967990e390 175 x1_hat_estimat(axm,aym,azm,u3,u2,Alpha);
YCTung 4:b0967990e390 176 m_x1_hat(mx,my,mz,u3,u2,Alpha);
YCTung 4:b0967990e390 177 m_x2_hat(mx,my,mz,u3,u1,Alpha);
YCTung 4:b0967990e390 178
YCTung 4:b0967990e390 179 if(cosroll != 0)
YCTung 4:b0967990e390 180 {
YCTung 7:b8413464d111 181 if(roll_angle >= 0.1745f || roll_angle <= -0.1745f)fall_down = 1;
YCTung 7:b8413464d111 182 else fall_down = 0;
YCTung 7:b8413464d111 183
YCTung 4:b0967990e390 184 droll_angle = lpf(u1, droll_angle_old, 62.8);
YCTung 4:b0967990e390 185 droll_angle_old = droll_angle;
YCTung 4:b0967990e390 186 dyaw_angle = lpf(u3/cosroll, dyaw_angle_old, 3.1416);
YCTung 4:b0967990e390 187 dyaw_angle_old = dyaw_angle;
YCTung 4:b0967990e390 188 // m_yaw_fusion();
YCTung 4:b0967990e390 189
YCTung 4:b0967990e390 190 ///cut off
YCTung 4:b0967990e390 191 if(m_sinyaw <= -1.0f)m_sinyaw = -1.0f;
YCTung 4:b0967990e390 192 else if(m_sinyaw >= 1.0f)m_sinyaw = 1.0f;
YCTung 4:b0967990e390 193 else ;
YCTung 4:b0967990e390 194 if(m_cosyaw <= -1.0f)m_cosyaw = -1.0f;
YCTung 4:b0967990e390 195 else if(m_cosyaw >= 1.0f)m_cosyaw = 1.0f;
YCTung 4:b0967990e390 196 else ;
YCTung 4:b0967990e390 197
YCTung 4:b0967990e390 198 ///Controller
YCTung 4:b0967990e390 199 if(sys_state == L_PD)
YCTung 4:b0967990e390 200 {
YCTung 4:b0967990e390 201 calc_PD(K_l, 0.0);
YCTung 4:b0967990e390 202 calc_Gamma(u,15,b_h);
YCTung 4:b0967990e390 203 gamma_rad = gamma_rad + L_PD_OFFSET;
YCTung 4:b0967990e390 204
YCTung 4:b0967990e390 205 dphi_hat = 0.0;
YCTung 4:b0967990e390 206 phi_hat = 0.0;
YCTung 9:159bad8bae03 207 // yaw_angle = yaw_angle_old + dyaw_angle * sample_time;
YCTung 9:159bad8bae03 208 // yaw_angle_old = yaw_angle;
YCTung 4:b0967990e390 209 }
YCTung 4:b0967990e390 210 else if(sys_state == M_PD)
YCTung 4:b0967990e390 211 {
YCTung 4:b0967990e390 212 calc_PD(K_m, 0.0);
YCTung 4:b0967990e390 213 calc_Gamma(u,15,b_h);
YCTung 4:b0967990e390 214
YCTung 4:b0967990e390 215 dphi_hat = 0.0;
YCTung 4:b0967990e390 216 phi_hat = 0.0;
YCTung 9:159bad8bae03 217 // yaw_angle = yaw_angle_old + dyaw_angle * sample_time;
YCTung 9:159bad8bae03 218 // yaw_angle_old = yaw_angle;
YCTung 4:b0967990e390 219
YCTung 4:b0967990e390 220 count2ztc_m++;
YCTung 4:b0967990e390 221 if(count2ztc_m >= CNT2ZTCm){sys_state = M_ZTC;}
YCTung 4:b0967990e390 222 }
YCTung 4:b0967990e390 223 else if(sys_state == M_ZTC)
YCTung 4:b0967990e390 224 {
YCTung 4:b0967990e390 225 if(count2straight < CNT2STRT){count2straight++;}
YCTung 4:b0967990e390 226 else{gamma_ref = 0.0; roll_ref = 0.0;}
YCTung 4:b0967990e390 227 calc_PD(K_m, phi_hat);
YCTung 4:b0967990e390 228 calc_Phi(K_mphi);
YCTung 4:b0967990e390 229 calc_Gamma(u,15,b_h);
YCTung 4:b0967990e390 230
YCTung 9:159bad8bae03 231 // yaw_angle = yaw_angle_old + dyaw_angle * sample_time;
YCTung 9:159bad8bae03 232 // yaw_angle_old = yaw_angle;
YCTung 4:b0967990e390 233 }
YCTung 4:b0967990e390 234 else if(sys_state == H_PD)
YCTung 4:b0967990e390 235 {
YCTung 4:b0967990e390 236 calc_PD(K_h,0.0);
YCTung 4:b0967990e390 237 calc_Gamma(u,15,b_h);
YCTung 4:b0967990e390 238
YCTung 4:b0967990e390 239 dphi_hat = 0.0;
YCTung 4:b0967990e390 240 phi_hat = 0.0;
YCTung 9:159bad8bae03 241 // yaw_angle = yaw_angle_old + dyaw_angle * sample_time;
YCTung 9:159bad8bae03 242 // yaw_angle_old = yaw_angle;
YCTung 4:b0967990e390 243
YCTung 4:b0967990e390 244 // count2ztc_h++;
YCTung 4:b0967990e390 245 if(count2ztc_h >= CNT2ZTCh){sys_state = H_ZTC;}
YCTung 4:b0967990e390 246 }
YCTung 4:b0967990e390 247 else if(sys_state == H_ZTC)
YCTung 4:b0967990e390 248 {
YCTung 4:b0967990e390 249 if(count2straight < CNT2STRT){count2straight++;}
YCTung 4:b0967990e390 250 else{gamma_ref = 0.0; roll_ref = 0.0;}
YCTung 4:b0967990e390 251 calc_PD(K_h, phi_hat);
YCTung 4:b0967990e390 252 calc_Phi(K_hphi);
YCTung 4:b0967990e390 253 calc_Gamma(u,15,b_h);
YCTung 4:b0967990e390 254
YCTung 9:159bad8bae03 255 // yaw_angle = yaw_angle_old + dyaw_angle * sample_time;
YCTung 9:159bad8bae03 256 // yaw_angle_old = yaw_angle;
YCTung 4:b0967990e390 257 }
YCTung 4:b0967990e390 258 else
YCTung 4:b0967990e390 259 {
YCTung 4:b0967990e390 260 u = 0.0;
YCTung 5:2290732b2782 261 // calc_PD(K_l, 0.0);
YCTung 4:b0967990e390 262 calc_Gamma(u,15,b_h);
YCTung 4:b0967990e390 263 dphi_hat = 0.0;
YCTung 4:b0967990e390 264 phi_hat = 0.0;
YCTung 4:b0967990e390 265 yaw_angle = 0.0;
YCTung 4:b0967990e390 266 yaw_angle_old = 0.0;
YCTung 4:b0967990e390 267 }
YCTung 4:b0967990e390 268
YCTung 4:b0967990e390 269 ///Anti-Windup
YCTung 4:b0967990e390 270 anti_wdup();
YCTung 4:b0967990e390 271
YCTung 4:b0967990e390 272 // calc_Gamma(u,15,b_h);
YCTung 4:b0967990e390 273 if(gamma_rad > 0.349f)gamma_rad = 0.349f;
YCTung 4:b0967990e390 274 else if(gamma_rad < -0.349f)gamma_rad = -0.349f;
YCTung 4:b0967990e390 275 else ;
YCTung 4:b0967990e390 276 // 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 277
YCTung 4:b0967990e390 278 ///Show results or Send datas
YCTung 4:b0967990e390 279 gamma_degree = (short int)(gamma_rad*114.59f);
YCTung 4:b0967990e390 280 if(gamma_degree > 40)gamma_degree = 40;
YCTung 4:b0967990e390 281 else if(gamma_degree < -40)gamma_degree = -40;
YCTung 4:b0967990e390 282 else ;
YCTung 4:b0967990e390 283
YCTung 4:b0967990e390 284 ///Steering
YCTung 4:b0967990e390 285 if(s == 1)
YCTung 4:b0967990e390 286 {
YCTung 4:b0967990e390 287 if(state_count > COUN2_HANDLE_START)
YCTung 4:b0967990e390 288 {
YCTung 4:b0967990e390 289 lookuptable_steering( gamma_degree );
YCTung 4:b0967990e390 290 }
YCTung 4:b0967990e390 291 else
YCTung 4:b0967990e390 292 {
YCTung 4:b0967990e390 293 lookuptable_steering( gamma_degree + HANDLE_START_BAL);
YCTung 4:b0967990e390 294 }
YCTung 4:b0967990e390 295 }
YCTung 4:b0967990e390 296 else
YCTung 4:b0967990e390 297 {
YCTung 4:b0967990e390 298 if(c == FIRST_POS)
YCTung 4:b0967990e390 299 {
YCTung 4:b0967990e390 300 if(state_change > 0)
YCTung 4:b0967990e390 301 {
YCTung 4:b0967990e390 302 lookuptable_steering(HANDLE_STOP);
YCTung 4:b0967990e390 303 }
YCTung 4:b0967990e390 304 else
YCTung 4:b0967990e390 305 {
YCTung 5:2290732b2782 306 if(!resetting)
YCTung 5:2290732b2782 307 {
YCTung 5:2290732b2782 308 lookuptable_steering(HANDLE_START);
YCTung 5:2290732b2782 309 // lookuptable_steering(HANDLE_STRAIGHT);
YCTung 5:2290732b2782 310 // lookuptable_steering( gamma_degree );
YCTung 5:2290732b2782 311 }
YCTung 5:2290732b2782 312 else ;
YCTung 4:b0967990e390 313 }
YCTung 4:b0967990e390 314 }
YCTung 4:b0967990e390 315 else lookuptable_steering( gamma_degree + HANDLE_START_BAL);
YCTung 4:b0967990e390 316 }
YCTung 4:b0967990e390 317
YCTung 4:b0967990e390 318 ///print file
YCTung 4:b0967990e390 319 if(sys_state >= 1)
YCTung 4:b0967990e390 320 {
YCTung 4:b0967990e390 321 count_isr++;
YCTung 4:b0967990e390 322 T_total = (float)sample_time*count_isr;
YCTung 4:b0967990e390 323
YCTung 4:b0967990e390 324 ///Integrate Ax to get Vx
YCTung 9:159bad8bae03 325 // Vx = Vx_old - axm_f*sample_time;
YCTung 9:159bad8bae03 326 // Vx_old = Vx;
YCTung 4:b0967990e390 327 // if(Vx >= v_low && sys_state < M_PD){pedal_state = 2; sys_state = M_PD; gamma_ref = 0.0;}
YCTung 4:b0967990e390 328
YCTung 7:b8413464d111 329 fprintf_data(1);
YCTung 4:b0967990e390 330 }
YCTung 4:b0967990e390 331 }
YCTung 4:b0967990e390 332 else
YCTung 4:b0967990e390 333 {
YCTung 4:b0967990e390 334 roll_fusion(0,0,0,0,0,1);
YCTung 4:b0967990e390 335 gamma_degree = 0;
YCTung 4:b0967990e390 336 }
YCTung 0:830ddddc129f 337 }
YCTung 0:830ddddc129f 338
YCTung 0:830ddddc129f 339 void timer2_interrupt(void)
YCTung 0:830ddddc129f 340 {
YCTung 4:b0967990e390 341 // pin_pc3 = !pin_pc3;
YCTung 3:197b748a397a 342 if(s == 1) // bluetooth start
YCTung 3:197b748a397a 343 {
YCTung 3:197b748a397a 344 i ++;
YCTung 3:197b748a397a 345 if(i == Z_PEDAL_DIVIDER && state_change == 0) //start
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 + TOT_STOP_POS)){c = FIRST_POS + TOT_STOP_POS + TOT_STOP_POS - 1;}
YCTung 3:197b748a397a 351 else if(c >= TOT_FOOT_POS){c = 0;}
YCTung 3:197b748a397a 352 state_count++;
YCTung 3:197b748a397a 353 }
YCTung 3:197b748a397a 354 else if(i == L_PEDAL_DIVIDER && state_change == 1) //low_v
YCTung 3:197b748a397a 355 {
YCTung 3:197b748a397a 356 i = 0;
YCTung 3:197b748a397a 357 lookuptable_pedaling(c);
YCTung 3:197b748a397a 358 c++;
YCTung 3:197b748a397a 359 if(c == FIRST_POS){c = FIRST_POS + TOT_STOP_POS;}
YCTung 3:197b748a397a 360 else if(c >= TOT_FOOT_POS){c = 0;}
YCTung 3:197b748a397a 361 state_count++;
YCTung 3:197b748a397a 362 }
YCTung 3:197b748a397a 363 else if(i == M_PEDAL_DIVIDER && state_change == 2) //mid_v
YCTung 3:197b748a397a 364 {
YCTung 3:197b748a397a 365 i = 0;
YCTung 3:197b748a397a 366 lookuptable_pedaling(c);
YCTung 3:197b748a397a 367 c++;
YCTung 3:197b748a397a 368 if(c == FIRST_POS){c = FIRST_POS + TOT_STOP_POS;}
YCTung 3:197b748a397a 369 else if(c >= TOT_FOOT_POS){c = 0;}
YCTung 3:197b748a397a 370 state_count++;
YCTung 3:197b748a397a 371 }
YCTung 3:197b748a397a 372 else if(i == M_PEDAL_DIVIDER && state_change == 3) //high_v
YCTung 3:197b748a397a 373 {
YCTung 3:197b748a397a 374 i = 0;
YCTung 3:197b748a397a 375 lookuptable_pedaling(c);
YCTung 3:197b748a397a 376 c++;
YCTung 3:197b748a397a 377 if(c == FIRST_POS){c = FIRST_POS + TOT_STOP_POS;}
YCTung 3:197b748a397a 378 else if(c >= TOT_FOOT_POS){c = 0;}
YCTung 3:197b748a397a 379 }
YCTung 3:197b748a397a 380
YCTung 3:197b748a397a 381 if(state_count == COUN2_LOW_V)
YCTung 3:197b748a397a 382 {
YCTung 3:197b748a397a 383 state_change = 1;
YCTung 3:197b748a397a 384 }
YCTung 3:197b748a397a 385 else if(state_count == COUN2_MID_V)
YCTung 3:197b748a397a 386 {
YCTung 3:197b748a397a 387 state_change = 2;
YCTung 3:197b748a397a 388 }
YCTung 3:197b748a397a 389 else if(state_count == COUN2_HIGH_V)
YCTung 3:197b748a397a 390 {
YCTung 3:197b748a397a 391 if(state_change == 2)
YCTung 3:197b748a397a 392 {
YCTung 5:2290732b2782 393 pc.printf("m\r\n");
YCTung 5:2290732b2782 394 // BT.printf("m\r\n");
YCTung 3:197b748a397a 395 pedal_state = 2;
YCTung 3:197b748a397a 396 sys_state = M_PD;
YCTung 3:197b748a397a 397 gamma_ref = 0.0;
YCTung 3:197b748a397a 398 if(count2ztc_m >= CNT2ZTCm)
YCTung 3:197b748a397a 399 {
YCTung 3:197b748a397a 400 sys_state = M_ZTC;
YCTung 3:197b748a397a 401 }
YCTung 3:197b748a397a 402 }
YCTung 3:197b748a397a 403 state_change = 3;
YCTung 3:197b748a397a 404 }
YCTung 3:197b748a397a 405 }
YCTung 3:197b748a397a 406 else //s is 0
YCTung 3:197b748a397a 407 {
YCTung 3:197b748a397a 408 if(c == FIRST_POS)
YCTung 3:197b748a397a 409 {
YCTung 3:197b748a397a 410 if(state_change > 0)
YCTung 3:197b748a397a 411 {
YCTung 3:197b748a397a 412 stop_pos();
YCTung 3:197b748a397a 413 brake_count++;
YCTung 3:197b748a397a 414 if(brake_count >= 3*244)
YCTung 3:197b748a397a 415 {
YCTung 3:197b748a397a 416 state_change = 0;
YCTung 3:197b748a397a 417 brake_count = 0;
YCTung 3:197b748a397a 418 }
YCTung 3:197b748a397a 419 }
YCTung 3:197b748a397a 420 else
YCTung 3:197b748a397a 421 {
YCTung 5:2290732b2782 422 reset_pos();
YCTung 3:197b748a397a 423 }
YCTung 3:197b748a397a 424 }
YCTung 3:197b748a397a 425 else
YCTung 3:197b748a397a 426 {
YCTung 3:197b748a397a 427 i++;
YCTung 3:197b748a397a 428 if(i == M_PEDAL_DIVIDER)
YCTung 3:197b748a397a 429 {
YCTung 3:197b748a397a 430 i = 0;
YCTung 3:197b748a397a 431 lookuptable_pedaling(c);
YCTung 3:197b748a397a 432 c++;
YCTung 3:197b748a397a 433 if(c > FIRST_POS && c < (FIRST_POS + TOT_STOP_POS)){c = FIRST_POS;}
YCTung 3:197b748a397a 434 else if(c >= TOT_FOOT_POS){c = 0;}
YCTung 3:197b748a397a 435 }
YCTung 3:197b748a397a 436 }
YCTung 3:197b748a397a 437 }
YCTung 2:ec30613f2b2b 438 }
YCTung 2:ec30613f2b2b 439
YCTung 7:b8413464d111 440 void fprintf_data(unsigned char mode)
YCTung 7:b8413464d111 441 {
YCTung 7:b8413464d111 442 if(mode == 0)
YCTung 7:b8413464d111 443 {
YCTung 7:b8413464d111 444 fprintf(dataPtr, "(not weighted)\nK_l: %f, %f\nK_m: %f, %f\nK_phi: %f, %f, %f\n", K_l[0], K_l[1], K_m[0], K_m[1], K_mphi[0], K_mphi[1], K_mphi[2]);
YCTung 7:b8413464d111 445 fprintf(dataPtr, "Gyro offset: %d %d %d\n", Gyro_axis_zero[0], Gyro_axis_zero[1], Gyro_axis_zero[2]);
YCTung 7:b8413464d111 446 fprintf(dataPtr, "Acce offset: %d %d %d\n", Acce_axis_zero[0], Acce_axis_zero[1], Acce_axis_zero[2]);
YCTung 7:b8413464d111 447 fprintf(dataPtr, "Magn offset: %d %d %d\n", Magn_axis_zero[0], Magn_axis_zero[1], Magn_axis_zero[2]);
YCTung 7:b8413464d111 448 fprintf(dataPtr, "sys_state\t");
YCTung 7:b8413464d111 449 fprintf(dataPtr, "time\t");
YCTung 7:b8413464d111 450 fprintf(dataPtr, "fall\t");
YCTung 7:b8413464d111 451 fprintf(dataPtr, "Roll\t");
YCTung 7:b8413464d111 452 // fprintf(dataPtr, "dRoll\t");
YCTung 7:b8413464d111 453 // fprintf(dataPtr, "dPhi_hat\t");
YCTung 7:b8413464d111 454 fprintf(dataPtr, "Phi_hat\t");
YCTung 9:159bad8bae03 455 fprintf(dataPtr, "dYaw\t");
YCTung 9:159bad8bae03 456 // fprintf(dataPtr, "Yaw\t");
YCTung 7:b8413464d111 457 // fprintf(dataPtr, "Pedal_step\t");
YCTung 7:b8413464d111 458 // fprintf(dataPtr, "Ctrl_out\t");
YCTung 7:b8413464d111 459 fprintf(dataPtr, "Gamma_ref\t");
YCTung 9:159bad8bae03 460 fprintf(dataPtr, "Gamma_rad\n");
YCTung 9:159bad8bae03 461 // fprintf(dataPtr, "Speed-X\n");
YCTung 7:b8413464d111 462 }
YCTung 7:b8413464d111 463 else
YCTung 7:b8413464d111 464 {
YCTung 7:b8413464d111 465 fprintf(dataPtr, "%d\t", sys_state);
YCTung 7:b8413464d111 466 fprintf(dataPtr, "%f\t", T_total);
YCTung 7:b8413464d111 467 fprintf(dataPtr, "%d\t", fall_down);
YCTung 7:b8413464d111 468 fprintf(dataPtr, "%f\t", roll_angle);
YCTung 7:b8413464d111 469 // fprintf(dataPtr, "%f\t", droll_angle);
YCTung 7:b8413464d111 470 // fprintf(dataPtr, "%f\t", dphi_hat);
YCTung 7:b8413464d111 471 fprintf(dataPtr, "%f\t", phi_hat);
YCTung 9:159bad8bae03 472 fprintf(dataPtr, "%f\t", dyaw_angle);
YCTung 9:159bad8bae03 473 // fprintf(dataPtr, "%f\t", yaw_angle);
YCTung 7:b8413464d111 474 // fprintf(dataPtr, "%d\t", pedal_step);
YCTung 7:b8413464d111 475 // fprintf(dataPtr, "%f\t", u);
YCTung 7:b8413464d111 476 fprintf(dataPtr, "%f\t", gamma_ref);
YCTung 9:159bad8bae03 477 fprintf(dataPtr, "%f\n", gamma_rad);
YCTung 9:159bad8bae03 478 // fprintf(dataPtr, "%f\n", Vx);
YCTung 7:b8413464d111 479 }
YCTung 7:b8413464d111 480 }
YCTung 7:b8413464d111 481
YCTung 2:ec30613f2b2b 482 void reset_offset(void)
YCTung 2:ec30613f2b2b 483 {
YCTung 2:ec30613f2b2b 484 pc.printf("Reseting gyro and accel-X offset...\r\n");
YCTung 2:ec30613f2b2b 485 resetting = 1;
YCTung 2:ec30613f2b2b 486 timeout.attach(&attimeout, 2.0f);
YCTung 2:ec30613f2b2b 487 while(resetting)
YCTung 2:ec30613f2b2b 488 {
YCTung 2:ec30613f2b2b 489 reset_gyro_offset();
YCTung 2:ec30613f2b2b 490 reset_acceX_offset();
YCTung 2:ec30613f2b2b 491 }
YCTung 2:ec30613f2b2b 492 timeout.detach();
YCTung 2:ec30613f2b2b 493 pc.printf("Done reseting.\r\n");
YCTung 2:ec30613f2b2b 494 // pc.printf("%d\r\n", Acce_axis_zero[0]);
YCTung 2:ec30613f2b2b 495 }
YCTung 2:ec30613f2b2b 496
YCTung 5:2290732b2782 497 void ready_to_go(void)
YCTung 5:2290732b2782 498 {
YCTung 5:2290732b2782 499 resetting = 1;
YCTung 5:2290732b2782 500 lookuptable_pedaling(FIRST_POS);
YCTung 5:2290732b2782 501 lookuptable_steering(HANDLE_STRAIGHT);
YCTung 5:2290732b2782 502 timeout.attach(&attimeout, 1.0f);
YCTung 5:2290732b2782 503 while(resetting)
YCTung 5:2290732b2782 504 {
YCTung 5:2290732b2782 505 wait_ms(100);
YCTung 5:2290732b2782 506 }
YCTung 5:2290732b2782 507 timeout.detach();
YCTung 5:2290732b2782 508 }
YCTung 5:2290732b2782 509
YCTung 2:ec30613f2b2b 510 void attimeout(void)
YCTung 2:ec30613f2b2b 511 {
YCTung 2:ec30613f2b2b 512 resetting = 0;
YCTung 5:2290732b2782 513 }
YCTung 5:2290732b2782 514 //void attimeout2(void)
YCTung 5:2290732b2782 515 //{
YCTung 5:2290732b2782 516 // preparing = 0;
YCTung 5:2290732b2782 517 //}