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@2:ec30613f2b2b, 2016-06-21 (annotated)

Committer:
YCTung
Date:
Tue Jun 21 18:47:22 2016 +0000
Revision:
2:ec30613f2b2b
Parent:
1:709be64ca63c
Child:
3:197b748a397a
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 1:709be64ca63c 7 /****************************
YCTung 1:709be64ca63c 8 Wiring Map
YCTung 0:830ddddc129f 9
YCTung 1:709be64ca63c 10 CK3 PC_10 | PC_11 MI3||re PC_9 | PC_8 rw
YCTung 1:709be64ca63c 11 MO3 PC_12 | PD_2 || |-----------------|
YCTung 2:ec30613f2b2b 12 3.3V | E5V ||rs |PB_8 D15 | PC_6 |TX6
YCTung 1:709be64ca63c 13 BOOT0 | GND ||ra |PB_9 D14 | PC_5 |
YCTung 1:709be64ca63c 14 |---------------| || |AVDD | U5V |
YCTung 1:709be64ca63c 15 |NC | NC | || |GND | NC |
YCTung 2:ec30613f2b2b 16 |NC | IOREF | ||rk |PA_5 D13 | PA_12|RX6
YCTung 1:709be64ca63c 17 |PA_13 | NRST | || |PA_6 D12 | PA_11| rl
YCTung 1:709be64ca63c 18 |PA_14 | 3.3V | || |PA_7 D11 | PB_12|
YCTung 1:709be64ca63c 19 |PA_15 | 5.0V | ||rb |PB_6 D10 | NC |
YCTung 1:709be64ca63c 20 |GND | GND | || |PC_7 D9 | GND |
YCTung 1:709be64ca63c 21 |PB_7 | GND | ||lb |PA_9 D8 | PB_2 |CSG
YCTung 1:709be64ca63c 22 |PC_13 | VIN | || |-----------------|
YCTung 1:709be64ca63c 23 |---------------| || |-----------------|
YCTung 1:709be64ca63c 24 PC_14 | NC ||ll |PA_8 D7 | PB_1 |CSX
YCTung 1:709be64ca63c 25 |---------------| ||lk |PB_10 D6 | PB_15|MO2
YCTung 1:709be64ca63c 26 |PC_15 | PA_0 A0| ||la |PB_4 D5 | PB_14|MI2
YCTung 1:709be64ca63c 27 |PH_0 | PA_1 A1| ||ls |PB_5 D4 | PB_13|CK2
YCTung 1:709be64ca63c 28 |PH_1 | PA_4 A2| ||le |PB_3 D3 | AGND |
YCTung 1:709be64ca63c 29 |VBAT | PB_0 A3| ||lw |PA_10 D2 | PC_4 |
YCTung 2:ec30613f2b2b 30 |PC_2 | PC_1 A4| ||TX2|PA_2 D1 | NC |
YCTung 2:ec30613f2b2b 31 |PC_3 | PC_0 A5| ||RX2|PA_3 D0 | NC |
YCTung 1:709be64ca63c 32 |---------------| || |-----------------|
YCTung 1:709be64ca63c 33 ****************************/
YCTung 0:830ddddc129f 34 AnalogIn analog_value(A5);//When using the ADC module, those pins connected to AnalogIn can't be used to output.
YCTung 0:830ddddc129f 35
YCTung 0:830ddddc129f 36 Ticker timer1;
YCTung 0:830ddddc129f 37 Ticker timer2;
YCTung 2:ec30613f2b2b 38 Timeout timeout;
YCTung 0:830ddddc129f 39
YCTung 0:830ddddc129f 40 //Serial pc(D1, D0) declared in SPI_9dSensor.h
YCTung 2:ec30613f2b2b 41 Serial BT(PC_6, PA_12);
YCTung 2:ec30613f2b2b 42
YCTung 2:ec30613f2b2b 43 SPI spi3(PC_12, PC_11, PC_10);
YCTung 0:830ddddc129f 44
YCTung 0:830ddddc129f 45 float meas, L_inver;
YCTung 2:ec30613f2b2b 46 bool resetting = 0;
YCTung 0:830ddddc129f 47
YCTung 0:830ddddc129f 48 void timer1_interrupt(void);
YCTung 0:830ddddc129f 49 void timer2_interrupt(void);
YCTung 2:ec30613f2b2b 50 void reset_offset(void);
YCTung 2:ec30613f2b2b 51 void attimeout(void);
YCTung 0:830ddddc129f 52
YCTung 0:830ddddc129f 53 int main() {
YCTung 0:830ddddc129f 54 setupServo();
YCTung 1:709be64ca63c 55 setup_spi();
YCTung 2:ec30613f2b2b 56 init_Sensors();
YCTung 2:ec30613f2b2b 57 BT.baud(115200);
YCTung 2:ec30613f2b2b 58
YCTung 2:ec30613f2b2b 59 reset_offset();
YCTung 0:830ddddc129f 60
YCTung 0:830ddddc129f 61 timer1.attach_us(&timer1_interrupt, 4000);//4.0ms interrupt period (250 Hz)
YCTung 0:830ddddc129f 62 timer2.attach_us(&timer2_interrupt, 4098);//4.098ms interrupt period (244 Hz)
YCTung 0:830ddddc129f 63
YCTung 2:ec30613f2b2b 64 spi3.format(8, 3);
YCTung 2:ec30613f2b2b 65 pc.printf("System ready.\r\n");
YCTung 2:ec30613f2b2b 66
YCTung 2:ec30613f2b2b 67 // pc.printf("\nAnalogIn example\r\n");
YCTung 0:830ddddc129f 68
YCTung 0:830ddddc129f 69 while(1) {
YCTung 2:ec30613f2b2b 70 // pc.printf("measure: %f V,\t%f cm\r\n", meas, 1/L_inver);
YCTung 2:ec30613f2b2b 71 // wait(0.1); // 200 ms
YCTung 0:830ddddc129f 72 }
YCTung 0:830ddddc129f 73 }
YCTung 0:830ddddc129f 74
YCTung 0:830ddddc129f 75 void timer1_interrupt(void)
YCTung 0:830ddddc129f 76 {
YCTung 0:830ddddc129f 77 meas = analog_value.read(); // Converts and read the analog input value (value from 0.0 to 1.0)
YCTung 0:830ddddc129f 78 meas = meas * 3.3f; // Change the value to be in the 0 to 3300 range
YCTung 0:830ddddc129f 79 L_inver = 0.0063f * meas - 0.005769f;
YCTung 0:830ddddc129f 80 }
YCTung 0:830ddddc129f 81
YCTung 0:830ddddc129f 82 void timer2_interrupt(void)
YCTung 0:830ddddc129f 83 {
YCTung 0:830ddddc129f 84 ;
YCTung 2:ec30613f2b2b 85 }
YCTung 2:ec30613f2b2b 86
YCTung 2:ec30613f2b2b 87 void reset_offset(void)
YCTung 2:ec30613f2b2b 88 {
YCTung 2:ec30613f2b2b 89 pc.printf("Reseting gyro and accel-X offset...\r\n");
YCTung 2:ec30613f2b2b 90 resetting = 1;
YCTung 2:ec30613f2b2b 91 timeout.attach(&attimeout, 2.0f);
YCTung 2:ec30613f2b2b 92 while(resetting)
YCTung 2:ec30613f2b2b 93 {
YCTung 2:ec30613f2b2b 94 reset_gyro_offset();
YCTung 2:ec30613f2b2b 95 reset_acceX_offset();
YCTung 2:ec30613f2b2b 96 }
YCTung 2:ec30613f2b2b 97 timeout.detach();
YCTung 2:ec30613f2b2b 98 pc.printf("Done reseting.\r\n");
YCTung 2:ec30613f2b2b 99 // pc.printf("%d\r\n", Acce_axis_zero[0]);
YCTung 2:ec30613f2b2b 100 }
YCTung 2:ec30613f2b2b 101
YCTung 2:ec30613f2b2b 102 void attimeout(void)
YCTung 2:ec30613f2b2b 103 {
YCTung 2:ec30613f2b2b 104 resetting = 0;
YCTung 0:830ddddc129f 105 }