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@3:197b748a397a, 2016-06-22 (annotated)

Committer:
YCTung
Date:
Wed Jun 22 04:35:18 2016 +0000
Revision:
3:197b748a397a
Parent:
2:ec30613f2b2b
Child:
4:b0967990e390
update

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 3:197b748a397a 30 // |PC_2 | PC_1 A4| ||TX2|PA_2 D1 | NC |
YCTung 3:197b748a397a 31 // |PC_3 | PC_0 A5| ||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 2:ec30613f2b2b 47 Serial BT(PC_6, PA_12);
YCTung 2:ec30613f2b2b 48
YCTung 2:ec30613f2b2b 49 SPI spi3(PC_12, PC_11, PC_10);
YCTung 0:830ddddc129f 50
YCTung 3:197b748a397a 51 //**** Variables from Arduino ****//
YCTung 3:197b748a397a 52 int counter = 0;
YCTung 3:197b748a397a 53 int i = 0;
YCTung 3:197b748a397a 54 int8_t c = FIRST_POS;
YCTung 3:197b748a397a 55 bool s = 0;
YCTung 3:197b748a397a 56 int fusion_init = 0;
YCTung 3:197b748a397a 57 int state_count = 0;
YCTung 3:197b748a397a 58 uint8_t state_change = 0;
YCTung 3:197b748a397a 59 uint16_t brake_count = 0;
YCTung 3:197b748a397a 60 float V_meas = 0;
YCTung 3:197b748a397a 61 float L_inver = 0;
YCTung 3:197b748a397a 62 bool toggle = 0;
YCTung 3:197b748a397a 63 //********************************//
YCTung 3:197b748a397a 64
YCTung 3:197b748a397a 65 //***** Variables from RasPi *****//
YCTung 3:197b748a397a 66 char BTCharR = 0;
YCTung 3:197b748a397a 67
YCTung 3:197b748a397a 68 float Vx = 0.0;
YCTung 3:197b748a397a 69 float Vx_old = 0.0;
YCTung 3:197b748a397a 70
YCTung 3:197b748a397a 71 unsigned char pedal_state = 0;
YCTung 3:197b748a397a 72 unsigned char sys_state = S_S;
YCTung 3:197b748a397a 73 unsigned int count2ztc_h = 0;
YCTung 3:197b748a397a 74 unsigned int count2ztc_m = 0;
YCTung 3:197b748a397a 75 unsigned int count2straight = 0;
YCTung 3:197b748a397a 76 unsigned int count_isr = 0;
YCTung 3:197b748a397a 77 float T_total = 0.0;
YCTung 3:197b748a397a 78 //********************************//
YCTung 3:197b748a397a 79
YCTung 2:ec30613f2b2b 80 bool resetting = 0;
YCTung 0:830ddddc129f 81
YCTung 0:830ddddc129f 82 void timer1_interrupt(void);
YCTung 0:830ddddc129f 83 void timer2_interrupt(void);
YCTung 2:ec30613f2b2b 84 void reset_offset(void);
YCTung 2:ec30613f2b2b 85 void attimeout(void);
YCTung 0:830ddddc129f 86
YCTung 0:830ddddc129f 87 int main() {
YCTung 0:830ddddc129f 88 setupServo();
YCTung 1:709be64ca63c 89 setup_spi();
YCTung 2:ec30613f2b2b 90 init_Sensors();
YCTung 2:ec30613f2b2b 91 BT.baud(115200);
YCTung 2:ec30613f2b2b 92
YCTung 2:ec30613f2b2b 93 reset_offset();
YCTung 0:830ddddc129f 94
YCTung 0:830ddddc129f 95 timer1.attach_us(&timer1_interrupt, 4000);//4.0ms interrupt period (250 Hz)
YCTung 0:830ddddc129f 96 timer2.attach_us(&timer2_interrupt, 4098);//4.098ms interrupt period (244 Hz)
YCTung 0:830ddddc129f 97
YCTung 2:ec30613f2b2b 98 spi3.format(8, 3);
YCTung 2:ec30613f2b2b 99 pc.printf("System ready.\r\n");
YCTung 2:ec30613f2b2b 100
YCTung 3:197b748a397a 101 reset_pos();
YCTung 3:197b748a397a 102 lookuptable_steering(HANDLE_START);
YCTung 0:830ddddc129f 103
YCTung 3:197b748a397a 104 while(!interrupt)
YCTung 3:197b748a397a 105 {
YCTung 3:197b748a397a 106 if(BT.readable())
YCTung 3:197b748a397a 107 {
YCTung 3:197b748a397a 108 BTCharR = BT.getc();
YCTung 3:197b748a397a 109 switch(BTCharR)
YCTung 3:197b748a397a 110 {
YCTung 3:197b748a397a 111 case 's': lookuptable_pedaling(FIRST_POS);
YCTung 3:197b748a397a 112 lookuptable_steering(HANDLE_STRAIGHT);
YCTung 3:197b748a397a 113 wait(1.5f);
YCTung 3:197b748a397a 114 pedal_state = 1; sys_state = L_PD; gamma_ref = 0.0;
YCTung 3:197b748a397a 115 s = 1; break; ///start
YCTung 3:197b748a397a 116 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 117 s = 0; state_count = 0; break; ///stop
YCTung 3:197b748a397a 118 // 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 119 // 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 120 case 'l': gamma_ref = DLT_GAMMA_REF; roll_ref = 0.0; count2straight = 0; break; ///left
YCTung 3:197b748a397a 121 case 'r': gamma_ref = -DLT_GAMMA_REF; roll_ref = 0.0; count2straight = 0; break; ///right
YCTung 3:197b748a397a 122 case 'f': gamma_ref = 0.0; roll_ref = 0.0; break; ///forward
YCTung 3:197b748a397a 123 case 'c': gamma_ref = 0.0; reset_offset(); break; ///clear
YCTung 3:197b748a397a 124 default: break;
YCTung 3:197b748a397a 125 }
YCTung 3:197b748a397a 126 BTCharR = 0;
YCTung 3:197b748a397a 127 }
YCTung 3:197b748a397a 128
YCTung 3:197b748a397a 129 L_inver = 0.0063f * V_meas - 0.005769f;
YCTung 0:830ddddc129f 130 }
YCTung 0:830ddddc129f 131 }
YCTung 0:830ddddc129f 132
YCTung 0:830ddddc129f 133 void timer1_interrupt(void)
YCTung 0:830ddddc129f 134 {
YCTung 3:197b748a397a 135 V_meas = (analog_value.read() * 3.3f) * IR_FILT_CONST + V_meas * (1.0f - IR_FILT_CONST);
YCTung 0:830ddddc129f 136 }
YCTung 0:830ddddc129f 137
YCTung 0:830ddddc129f 138 void timer2_interrupt(void)
YCTung 0:830ddddc129f 139 {
YCTung 3:197b748a397a 140 if(s == 1) // bluetooth start
YCTung 3:197b748a397a 141 {
YCTung 3:197b748a397a 142 i ++;
YCTung 3:197b748a397a 143 if(i == Z_PEDAL_DIVIDER && state_change == 0) //start
YCTung 3:197b748a397a 144 {
YCTung 3:197b748a397a 145 i = 0;
YCTung 3:197b748a397a 146 lookuptable_pedaling(c);
YCTung 3:197b748a397a 147 c++;
YCTung 3:197b748a397a 148 if(c == (FIRST_POS + TOT_STOP_POS)){c = FIRST_POS + TOT_STOP_POS + TOT_STOP_POS - 1;}
YCTung 3:197b748a397a 149 else if(c >= TOT_FOOT_POS){c = 0;}
YCTung 3:197b748a397a 150 state_count++;
YCTung 3:197b748a397a 151 }
YCTung 3:197b748a397a 152 else if(i == L_PEDAL_DIVIDER && state_change == 1) //low_v
YCTung 3:197b748a397a 153 {
YCTung 3:197b748a397a 154 i = 0;
YCTung 3:197b748a397a 155 lookuptable_pedaling(c);
YCTung 3:197b748a397a 156 c++;
YCTung 3:197b748a397a 157 if(c == FIRST_POS){c = FIRST_POS + TOT_STOP_POS;}
YCTung 3:197b748a397a 158 else if(c >= TOT_FOOT_POS){c = 0;}
YCTung 3:197b748a397a 159 state_count++;
YCTung 3:197b748a397a 160 }
YCTung 3:197b748a397a 161 else if(i == M_PEDAL_DIVIDER && state_change == 2) //mid_v
YCTung 3:197b748a397a 162 {
YCTung 3:197b748a397a 163 i = 0;
YCTung 3:197b748a397a 164 lookuptable_pedaling(c);
YCTung 3:197b748a397a 165 c++;
YCTung 3:197b748a397a 166 if(c == FIRST_POS){c = FIRST_POS + TOT_STOP_POS;}
YCTung 3:197b748a397a 167 else if(c >= TOT_FOOT_POS){c = 0;}
YCTung 3:197b748a397a 168 state_count++;
YCTung 3:197b748a397a 169 }
YCTung 3:197b748a397a 170 else if(i == M_PEDAL_DIVIDER && state_change == 3) //high_v
YCTung 3:197b748a397a 171 {
YCTung 3:197b748a397a 172 i = 0;
YCTung 3:197b748a397a 173 lookuptable_pedaling(c);
YCTung 3:197b748a397a 174 c++;
YCTung 3:197b748a397a 175 if(c == FIRST_POS){c = FIRST_POS + TOT_STOP_POS;}
YCTung 3:197b748a397a 176 else if(c >= TOT_FOOT_POS){c = 0;}
YCTung 3:197b748a397a 177 }
YCTung 3:197b748a397a 178
YCTung 3:197b748a397a 179 if(state_count == COUN2_LOW_V)
YCTung 3:197b748a397a 180 {
YCTung 3:197b748a397a 181 state_change = 1;
YCTung 3:197b748a397a 182 }
YCTung 3:197b748a397a 183 else if(state_count == COUN2_MID_V)
YCTung 3:197b748a397a 184 {
YCTung 3:197b748a397a 185 state_change = 2;
YCTung 3:197b748a397a 186 }
YCTung 3:197b748a397a 187 else if(state_count == COUN2_HIGH_V)
YCTung 3:197b748a397a 188 {
YCTung 3:197b748a397a 189 if(state_change == 2)
YCTung 3:197b748a397a 190 {
YCTung 3:197b748a397a 191 // pc.printf("\nm");
YCTung 3:197b748a397a 192 pedal_state = 2;
YCTung 3:197b748a397a 193 sys_state = M_PD;
YCTung 3:197b748a397a 194 gamma_ref = 0.0;
YCTung 3:197b748a397a 195 if(count2ztc_m >= CNT2ZTCm)
YCTung 3:197b748a397a 196 {
YCTung 3:197b748a397a 197 sys_state = M_ZTC;
YCTung 3:197b748a397a 198 }
YCTung 3:197b748a397a 199 }
YCTung 3:197b748a397a 200 state_change = 3;
YCTung 3:197b748a397a 201 }
YCTung 3:197b748a397a 202 }
YCTung 3:197b748a397a 203 else //s is 0
YCTung 3:197b748a397a 204 {
YCTung 3:197b748a397a 205 if(c == FIRST_POS)
YCTung 3:197b748a397a 206 {
YCTung 3:197b748a397a 207 if(state_change > 0)
YCTung 3:197b748a397a 208 {
YCTung 3:197b748a397a 209 stop_pos();
YCTung 3:197b748a397a 210 brake_count++;
YCTung 3:197b748a397a 211 if(brake_count >= 3*244)
YCTung 3:197b748a397a 212 {
YCTung 3:197b748a397a 213 state_change = 0;
YCTung 3:197b748a397a 214 brake_count = 0;
YCTung 3:197b748a397a 215 }
YCTung 3:197b748a397a 216 }
YCTung 3:197b748a397a 217 else
YCTung 3:197b748a397a 218 {
YCTung 3:197b748a397a 219 reset_pos();
YCTung 3:197b748a397a 220 }
YCTung 3:197b748a397a 221 }
YCTung 3:197b748a397a 222 else
YCTung 3:197b748a397a 223 {
YCTung 3:197b748a397a 224 i++;
YCTung 3:197b748a397a 225 if(i == M_PEDAL_DIVIDER)
YCTung 3:197b748a397a 226 {
YCTung 3:197b748a397a 227 i = 0;
YCTung 3:197b748a397a 228 lookuptable_pedaling(c);
YCTung 3:197b748a397a 229 c++;
YCTung 3:197b748a397a 230 if(c > FIRST_POS && c < (FIRST_POS + TOT_STOP_POS)){c = FIRST_POS;}
YCTung 3:197b748a397a 231 else if(c >= TOT_FOOT_POS){c = 0;}
YCTung 3:197b748a397a 232 }
YCTung 3:197b748a397a 233 }
YCTung 3:197b748a397a 234 }
YCTung 2:ec30613f2b2b 235 }
YCTung 2:ec30613f2b2b 236
YCTung 2:ec30613f2b2b 237 void reset_offset(void)
YCTung 2:ec30613f2b2b 238 {
YCTung 2:ec30613f2b2b 239 pc.printf("Reseting gyro and accel-X offset...\r\n");
YCTung 2:ec30613f2b2b 240 resetting = 1;
YCTung 2:ec30613f2b2b 241 timeout.attach(&attimeout, 2.0f);
YCTung 2:ec30613f2b2b 242 while(resetting)
YCTung 2:ec30613f2b2b 243 {
YCTung 2:ec30613f2b2b 244 reset_gyro_offset();
YCTung 2:ec30613f2b2b 245 reset_acceX_offset();
YCTung 2:ec30613f2b2b 246 }
YCTung 2:ec30613f2b2b 247 timeout.detach();
YCTung 2:ec30613f2b2b 248 pc.printf("Done reseting.\r\n");
YCTung 2:ec30613f2b2b 249 // pc.printf("%d\r\n", Acce_axis_zero[0]);
YCTung 2:ec30613f2b2b 250 }
YCTung 2:ec30613f2b2b 251
YCTung 2:ec30613f2b2b 252 void attimeout(void)
YCTung 2:ec30613f2b2b 253 {
YCTung 2:ec30613f2b2b 254 resetting = 0;
YCTung 0:830ddddc129f 255 }