Diff: Sensorplate/main.cpp

Revision:

53:54c882995514

Parent:

51:69087c44e8ac

Child:

55:b74e7076d7a0

--- a/Sensorplate/main.cpp	Thu Feb 01 18:05:46 2018 +0000
+++ b/Sensorplate/main.cpp	Mon Feb 12 10:23:58 2018 +0000
@@ -56,7 +56,7 @@
 //neopixel::PixelArray indicator_LEDs(PA_7);
 PixelArray px(WS2812_BUF);
 //WS2812 ws(PA_7, WS2812_BUF, 3, 9, 9, 6);
-WS2812 ws(PA_7, WS2812_BUF, 3, 35, 12, 4);
+WS2812 ws(PA_7, WS2812_BUF, 3, 9, 9, 6);
 
 PwmOut lock_feedback_LED(PB_13);        //(PB_1);                                                      // Declaration of pulse with modulation outputs.
 PwmOut mute_feedback_LED(PB_14);        //(PB_15);
@@ -323,6 +323,8 @@
         if (test_mode == 1) {
             usb_serial.printf("!,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,\n", piezo_resistive_array[0], piezo_resistive_array[1], piezo_resistive_array[2], piezo_resistive_array[3], piezo_resistive_array[4], piezo_resistive_array[5], piezo_resistive_array[6], piezo_resistive_array[7], piezo_electric_array[0], piezo_electric_array[1], piezo_electric_array[2], piezo_electric_array[3], piezo_electric_array[4]); // print all to serial port
         }
+    } else {
+        pi_serial.printf("!,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,%d,\n",0,0,0,0,0,0,0,0,0,0,0,0,0);
     }
 
 }
@@ -357,7 +359,7 @@
         calibration_flash--;
     }
     //for (int z=WS2812_BUF; z >= 0 ; z--) {
-        ws.write_offsets(px.getBuf(),0,0,0);
+    ws.write_offsets(px.getBuf(),0,0,0);
     //}
 }
 
@@ -555,7 +557,7 @@
     ws.useII(WS2812::GLOBAL);                                                       // use global intensity scaling
     set_intensity_LEDs();                                                           // Initialize intensity for user interface LED's and LED's shines to wall.
     colour_select_indicating_LED_wall(LED_colour);
-    
+
     speaker1 = 1;
     wait_ms(boot_delay_ms);                                                         // Wait to boot sensorplate first.
     speaker1 = 0;
@@ -660,9 +662,14 @@
             angle = accelerometer_sensorplate[2]*100;
             if(angle == 0) {
                 MPU6050_belt angle_device_sensorplate(PB_9, PB_8);
-               angle_device_sensorplate.getAccelero(accelerometer_sensorplate);
+                angle_device_sensorplate.getAccelero(accelerometer_sensorplate);
                 angle = accelerometer_sensorplate[2]*100;
             }
+            if((abs(accelerometer_sensorplate[0])+abs(accelerometer_sensorplate[1])+abs(accelerometer_sensorplate[2]))<= 6) {
+                for(uint8_t k = 0; k < 3; ++k) {
+                    accelerometer_sensorplate[k]=accelerometer_sensorplate[k]*2;
+                }
+            }
             angle_device_sensorplate.getGyro(gyroscope_sensorplate);                        // Get gyroscope data.
             if (test_mode == 1) {
                 usb_serial.printf("Loop time: %d ms\n",piezo_electric_sample_timer.read_ms());
@@ -675,11 +682,14 @@
             if (connection_test_sensorplate == 1) {
                 if (test_belt == 0) {                                                     // If statement for sending serial information sensorplate data when connection test is active.
                     pi_serial.printf("?,%f,%f,%f,%f,%f,%f,0,0,0,0,0,0,\n", accelerometer_sensorplate[0], accelerometer_sensorplate[1], accelerometer_sensorplate[2], gyroscope_sensorplate[0], gyroscope_sensorplate[1], gyroscope_sensorplate[2]);
-                    } else{
-                // Receiving order sensor information: 3 accelero sensors & 3 gyroscope sensors from sensorplate; 3 accelero sensors & 3 gyroscope sensors from belt. Is splitted in two parts - part 2/2.
-                pi_serial.printf("?,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,\n", accelerometer_sensorplate[0], accelerometer_sensorplate[1], accelerometer_sensorplate[2], gyroscope_sensorplate[0], gyroscope_sensorplate[1], gyroscope_sensorplate[2], accelerometer_reference_belt[0], accelerometer_reference_belt[1], accelerometer_reference_belt[2], gyroscope_reference_belt[0], gyroscope_reference_belt[1], gyroscope_reference_belt[2]);
+                } else {
+                    // Receiving order sensor information: 3 accelero sensors & 3 gyroscope sensors from sensorplate; 3 accelero sensors & 3 gyroscope sensors from belt. Is splitted in two parts - part 2/2.
+                    pi_serial.printf("?,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,\n", accelerometer_sensorplate[0], accelerometer_sensorplate[1], accelerometer_sensorplate[2], gyroscope_sensorplate[0], gyroscope_sensorplate[1], gyroscope_sensorplate[2], accelerometer_reference_belt[0], accelerometer_reference_belt[1], accelerometer_reference_belt[2], gyroscope_reference_belt[0], gyroscope_reference_belt[1], gyroscope_reference_belt[2]);
                 }
             } // binair print and convert in pi
+            else {
+                pi_serial.printf("?,%f,%f,%f,%f,%f,%f,0,0,0,0,0,0,\n",0,0,0,0,0,0);
+            }
 
             while(piezo_electric_sample_timer.read_us()<(3*(total_readout_cycle_time_us/5))) {}  // Wait untill 60% of cycle. Energy efficiency is not fine in this situation, correct if low energy is needed.