Chris LU
code of robot bike
Dependencies: SDFileSystem mbed
Fork of
Robot_Bicycle
by 
Chris LU
Diff: main.cpp
- Revision:
- 3:197b748a397a
- Parent:
- 2:ec30613f2b2b
- Child:
- 4:b0967990e390
--- a/main.cpp Tue Jun 21 18:47:22 2016 +0000
+++ b/main.cpp Wed Jun 22 04:35:18 2016 +0000
@@ -4,45 +4,79 @@
#include "SensorFusion.h"
#include "SPI_9dSensor.h"
#include "RobotBicycleConst.h"
-/****************************
-Wiring Map
+#include "ZTC.h"
+//************************************************//
+//**************** Wiring Map ********************//
+//CK3 PC_10 | PC_11 MI3||re PC_9 | PC_8 rw
+//MO3 PC_12 | PD_2 || |-----------------|
+// 3.3V | E5V ||rs |PB_8 D15 | PC_6 |TX6
+// BOOT0 | GND ||ra |PB_9 D14 | PC_5 |
+// |---------------| || |AVDD | U5V |
+// |NC | NC | || |GND | NC |
+// |NC | IOREF | ||rk |PA_5 D13 | PA_12|RX6
+// |PA_13 | NRST | || |PA_6 D12 | PA_11| rl
+// |PA_14 | 3.3V | || |PA_7 D11 | PB_12|
+// |PA_15 | 5.0V | ||rb |PB_6 D10 | NC |
+// |GND | GND | || |PC_7 D9 | GND |
+// |PB_7 | GND | ||lb |PA_9 D8 | PB_2 |CSG
+// |PC_13 | VIN | || |-----------------|
+// |---------------| || |-----------------|
+// PC_14 | NC ||ll |PA_8 D7 | PB_1 |CSX
+// |---------------| ||lk |PB_10 D6 | PB_15|MO2
+// |PC_15 | PA_0 A0| ||la |PB_4 D5 | PB_14|MI2
+// |PH_0 | PA_1 A1| ||ls |PB_5 D4 | PB_13|CK2
+// |PH_1 | PA_4 A2| ||le |PB_3 D3 | AGND |
+// |VBAT | PB_0 A3| ||lw |PA_10 D2 | PC_4 |
+// |PC_2 | PC_1 A4| ||TX2|PA_2 D1 | NC |
+// |PC_3 | PC_0 A5| ||RX2|PA_3 D0 | NC |
+// |---------------| || |-----------------|
+//************************************************//
-CK3 PC_10 | PC_11 MI3||re PC_9 | PC_8 rw
-MO3 PC_12 | PD_2 || |-----------------|
- 3.3V | E5V ||rs |PB_8 D15 | PC_6 |TX6
- BOOT0 | GND ||ra |PB_9 D14 | PC_5 |
- |---------------| || |AVDD | U5V |
- |NC | NC | || |GND | NC |
- |NC | IOREF | ||rk |PA_5 D13 | PA_12|RX6
- |PA_13 | NRST | || |PA_6 D12 | PA_11| rl
- |PA_14 | 3.3V | || |PA_7 D11 | PB_12|
- |PA_15 | 5.0V | ||rb |PB_6 D10 | NC |
- |GND | GND | || |PC_7 D9 | GND |
- |PB_7 | GND | ||lb |PA_9 D8 | PB_2 |CSG
- |PC_13 | VIN | || |-----------------|
- |---------------| || |-----------------|
- PC_14 | NC ||ll |PA_8 D7 | PB_1 |CSX
- |---------------| ||lk |PB_10 D6 | PB_15|MO2
- |PC_15 | PA_0 A0| ||la |PB_4 D5 | PB_14|MI2
- |PH_0 | PA_1 A1| ||ls |PB_5 D4 | PB_13|CK2
- |PH_1 | PA_4 A2| ||le |PB_3 D3 | AGND |
- |VBAT | PB_0 A3| ||lw |PA_10 D2 | PC_4 |
- |PC_2 | PC_1 A4| ||TX2|PA_2 D1 | NC |
- |PC_3 | PC_0 A5| ||RX2|PA_3 D0 | NC |
- |---------------| || |-----------------|
-****************************/
+#define IR_FILT_CONST 1.0f * 2.0f * 3.14159f / 250.0f
+
+#define CNT2ZTCm 250
+#define CNT2ZTCh 5
+#define CNT2STRT 1250
+
AnalogIn analog_value(A5);//When using the ADC module, those pins connected to AnalogIn can't be used to output.
Ticker timer1;
Ticker timer2;
Timeout timeout;
-//Serial pc(D1, D0) declared in SPI_9dSensor.h
Serial BT(PC_6, PA_12);
SPI spi3(PC_12, PC_11, PC_10);
-float meas, L_inver;
+//**** Variables from Arduino ****//
+int counter = 0;
+int i = 0;
+int8_t c = FIRST_POS;
+bool s = 0;
+int fusion_init = 0;
+int state_count = 0;
+uint8_t state_change = 0;
+uint16_t brake_count = 0;
+float V_meas = 0;
+float L_inver = 0;
+bool toggle = 0;
+//********************************//
+
+//***** Variables from RasPi *****//
+char BTCharR = 0;
+
+float Vx = 0.0;
+float Vx_old = 0.0;
+
+unsigned char pedal_state = 0;
+unsigned char sys_state = S_S;
+unsigned int count2ztc_h = 0;
+unsigned int count2ztc_m = 0;
+unsigned int count2straight = 0;
+unsigned int count_isr = 0;
+float T_total = 0.0;
+//********************************//
+
bool resetting = 0;
void timer1_interrupt(void);
@@ -64,24 +98,140 @@
spi3.format(8, 3);
pc.printf("System ready.\r\n");
-// pc.printf("\nAnalogIn example\r\n");
+ reset_pos();
+ lookuptable_steering(HANDLE_START);
- while(1) {
-// pc.printf("measure: %f V,\t%f cm\r\n", meas, 1/L_inver);
-// wait(0.1); // 200 ms
+ while(!interrupt)
+ {
+ if(BT.readable())
+ {
+ BTCharR = BT.getc();
+ switch(BTCharR)
+ {
+ case 's': lookuptable_pedaling(FIRST_POS);
+ lookuptable_steering(HANDLE_STRAIGHT);
+ wait(1.5f);
+ pedal_state = 1; sys_state = L_PD; gamma_ref = 0.0;
+ s = 1; break; ///start
+ 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;
+ s = 0; state_count = 0; break; ///stop
+// case 'm': pedal_state = 2; sys_state = M_PD; gamma_ref = 0.0; if(count2ztc_m >= CNT2ZTCm){sys_state = M_ZTC;} break;
+// case 'h': pedal_state = 3; sys_state = H_PD; gamma_ref = 0.0; if(count2ztc_h >= CNT2ZTCh){sys_state = H_ZTC;} break;
+ case 'l': gamma_ref = DLT_GAMMA_REF; roll_ref = 0.0; count2straight = 0; break; ///left
+ case 'r': gamma_ref = -DLT_GAMMA_REF; roll_ref = 0.0; count2straight = 0; break; ///right
+ case 'f': gamma_ref = 0.0; roll_ref = 0.0; break; ///forward
+ case 'c': gamma_ref = 0.0; reset_offset(); break; ///clear
+ default: break;
+ }
+ BTCharR = 0;
+ }
+
+ L_inver = 0.0063f * V_meas - 0.005769f;
}
}
void timer1_interrupt(void)
{
- meas = analog_value.read(); // Converts and read the analog input value (value from 0.0 to 1.0)
- meas = meas * 3.3f; // Change the value to be in the 0 to 3300 range
- L_inver = 0.0063f * meas - 0.005769f;
+ V_meas = (analog_value.read() * 3.3f) * IR_FILT_CONST + V_meas * (1.0f - IR_FILT_CONST);
}
void timer2_interrupt(void)
{
- ;
+ if(s == 1) // bluetooth start
+ {
+ i ++;
+ if(i == Z_PEDAL_DIVIDER && state_change == 0) //start
+ {
+ i = 0;
+ lookuptable_pedaling(c);
+ c++;
+ if(c == (FIRST_POS + TOT_STOP_POS)){c = FIRST_POS + TOT_STOP_POS + TOT_STOP_POS - 1;}
+ else if(c >= TOT_FOOT_POS){c = 0;}
+ state_count++;
+ }
+ else if(i == L_PEDAL_DIVIDER && state_change == 1) //low_v
+ {
+ i = 0;
+ lookuptable_pedaling(c);
+ c++;
+ if(c == FIRST_POS){c = FIRST_POS + TOT_STOP_POS;}
+ else if(c >= TOT_FOOT_POS){c = 0;}
+ state_count++;
+ }
+ else if(i == M_PEDAL_DIVIDER && state_change == 2) //mid_v
+ {
+ i = 0;
+ lookuptable_pedaling(c);
+ c++;
+ if(c == FIRST_POS){c = FIRST_POS + TOT_STOP_POS;}
+ else if(c >= TOT_FOOT_POS){c = 0;}
+ state_count++;
+ }
+ else if(i == M_PEDAL_DIVIDER && state_change == 3) //high_v
+ {
+ i = 0;
+ lookuptable_pedaling(c);
+ c++;
+ if(c == FIRST_POS){c = FIRST_POS + TOT_STOP_POS;}
+ else if(c >= TOT_FOOT_POS){c = 0;}
+ }
+
+ if(state_count == COUN2_LOW_V)
+ {
+ state_change = 1;
+ }
+ else if(state_count == COUN2_MID_V)
+ {
+ state_change = 2;
+ }
+ else if(state_count == COUN2_HIGH_V)
+ {
+ if(state_change == 2)
+ {
+// pc.printf("\nm");
+ pedal_state = 2;
+ sys_state = M_PD;
+ gamma_ref = 0.0;
+ if(count2ztc_m >= CNT2ZTCm)
+ {
+ sys_state = M_ZTC;
+ }
+ }
+ state_change = 3;
+ }
+ }
+ else //s is 0
+ {
+ if(c == FIRST_POS)
+ {
+ if(state_change > 0)
+ {
+ stop_pos();
+ brake_count++;
+ if(brake_count >= 3*244)
+ {
+ state_change = 0;
+ brake_count = 0;
+ }
+ }
+ else
+ {
+ reset_pos();
+ }
+ }
+ else
+ {
+ i++;
+ if(i == M_PEDAL_DIVIDER)
+ {
+ i = 0;
+ lookuptable_pedaling(c);
+ c++;
+ if(c > FIRST_POS && c < (FIRST_POS + TOT_STOP_POS)){c = FIRST_POS;}
+ else if(c >= TOT_FOOT_POS){c = 0;}
+ }
+ }
+ }
}
void reset_offset(void)