Diff: main.cpp

Revision:

2:ec30613f2b2b

Parent:

1:709be64ca63c

Child:

3:197b748a397a

--- a/main.cpp	Tue Jun 21 18:11:46 2016 +0000
+++ b/main.cpp	Tue Jun 21 18:47:22 2016 +0000
@@ -9,11 +9,11 @@
 
 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 |
+    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|
+   |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   |
@@ -27,34 +27,48 @@
    |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|   ||   |PA_2  D1  | NC   |
-   |PC_3  | PC_0 A5|   ||   |PA_3  D0  | NC   |
+   |PC_2  | PC_1 A4|   ||TX2|PA_2  D1  | NC   |
+   |PC_3  | PC_0 A5|   ||RX2|PA_3  D0  | NC   |
    |---------------|   ||   |-----------------|
 ****************************/
 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;
+bool resetting = 0;
 
 void timer1_interrupt(void);
 void timer2_interrupt(void);
+void reset_offset(void);
+void attimeout(void);
 
 int main() {
     setupServo();
     setup_spi();
+    init_Sensors();
+    BT.baud(115200);
+    
+    reset_offset();
     
     timer1.attach_us(&timer1_interrupt, 4000);//4.0ms interrupt period (250 Hz)
     timer2.attach_us(&timer2_interrupt, 4098);//4.098ms interrupt period (244 Hz)
     
-    pc.printf("\nAnalogIn example\r\n");
+    spi3.format(8, 3);
+    pc.printf("System ready.\r\n");
+    
+//    pc.printf("\nAnalogIn example\r\n");
     
     while(1) {
-        pc.printf("measure: %f V,\t%f cm\r\n", meas, 1/L_inver);
-        wait(0.1); // 200 ms
+//        pc.printf("measure: %f V,\t%f cm\r\n", meas, 1/L_inver);
+//        wait(0.1); // 200 ms
     }
 }
 
@@ -68,4 +82,24 @@
 void timer2_interrupt(void)
 {
     ;
+}
+
+void reset_offset(void)
+{
+    pc.printf("Reseting gyro and accel-X offset...\r\n");
+    resetting = 1;
+    timeout.attach(&attimeout, 2.0f);
+    while(resetting)
+    {
+        reset_gyro_offset();
+        reset_acceX_offset();
+    }
+    timeout.detach();
+    pc.printf("Done reseting.\r\n");
+//    pc.printf("%d\r\n", Acce_axis_zero[0]);
+}
+
+void attimeout(void)
+{
+    resetting = 0;
 }
\ No newline at end of file