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

Committer:
YCTung
Date:
Sun Jul 10 09:41:33 2016 +0000
Revision:
7:b8413464d111
Parent:
6:bd469c945e41
Child:
8:79ca11e0129d
print data to SD

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