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@13:51ef67cd4fd6, 2017-05-17 (annotated)

Committer:
cpul5338
Date:
Wed May 17 15:22:29 2017 +0000
Revision:
13:51ef67cd4fd6
Parent:
12:60593247555d 
template

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