Revision 2:9fe6f1e273b4, committed 2014-08-28

Comitter:

jskl

Date:

Thu Aug 28 07:50:06 2014 +0000

Parent:

1:f1ee978773d3

Commit message:

Fixed bugs in JSON format replies

Changed in this revision

--- a/GY80/GY80.cpp	Tue Aug 26 06:46:55 2014 +0000
+++ b/GY80/GY80.cpp	Thu Aug 28 07:50:06 2014 +0000
@@ -2,6 +2,7 @@
 
 Serial pc2(USBTX, USBRX);
 
+
 GY80::GY80() : Wire( SDA,SCL)
 {
     Wire.frequency(I2C_FREQ);
@@ -73,16 +74,19 @@
     buff[0] = 0x32; // Send address to read from
     Wire.write(ACCEL_ADDRESS, buff, 1);
 
-    int accel[3];
+    //int accel[3];
     if (Wire.read(ACCEL_ADDRESS, buff,6) == 0)  // All bytes received?
     {
-        accel[0] = (short) ((uint16_t) buff[1] << 8 | buff[0]);
-        accel[1] = (short) ((uint16_t) buff[3] << 8 | buff[2]);
-        accel[2] = (short) ((uint16_t) buff[5] << 8 | buff[4]);
+        accel_v[0] = (short) ((uint16_t) buff[1] << 8 | buff[0]);
+        accel_v[1] = (short) ((uint16_t) buff[3] << 8 | buff[2]);
+        accel_v[2] = (short) ((uint16_t) buff[5] << 8 | buff[4]);
     }
-    accel_v[0] = (accel[0] - ACCEL_X_OFFSET) * ACCEL_X_SCALE;
+    /*accel_v[0] = (accel[0] - ACCEL_X_OFFSET) * ACCEL_X_SCALE;
     accel_v[1] = (accel[1] - ACCEL_Y_OFFSET) * ACCEL_Y_SCALE;
-    accel_v[2] = (accel[2] - ACCEL_Z_OFFSET) * ACCEL_Z_SCALE;
+    accel_v[2] = (accel[2] - ACCEL_Z_OFFSET) * ACCEL_Z_SCALE;*/
+    /*accel_v[0] = accel[0]*0.5 + accel_v[0]*0.5;
+    accel_v[1] = accel[0]*0.5 + accel_v[0]*0.5;
+    accel_v[2] = accel[0]*0.5 + accel_v[0]*0.5;*/
 }
 
 
@@ -93,16 +97,16 @@
     buff[0] = 0xA8; // 0x28 | (1 << 7) Send address to read from 
     Wire.write(GYRO_ADDRESS, buff, 1);
     // Request 6 bytes
-    int gyro[3];
+    // int gyro[3];
     if (Wire.read(GYRO_ADDRESS, buff,6) == 0)  // All bytes received?
     {
-        gyro[0] = (short) ((uint16_t) buff[1] << 8 | buff[0]);
-        gyro[1] = (short) ((uint16_t) buff[3] << 8 | buff[2]);
-        gyro[2] = (short) ((uint16_t) buff[5] << 8 | buff[4]);
+        gyro_v[0] = (short) ((uint16_t) buff[1] << 8 | buff[0]);
+        gyro_v[1] = (short) ((uint16_t) buff[3] << 8 | buff[2]);
+        gyro_v[2] = (short) ((uint16_t) buff[5] << 8 | buff[4]);
     }
-    gyro_v[0] = DEG2RAD((gyro[0] - GYRO_X_OFFSET) * GYRO_GAIN_X); 
+    /*gyro_v[0] = DEG2RAD((gyro[0] - GYRO_X_OFFSET) * GYRO_GAIN_X); 
     gyro_v[1] = DEG2RAD((gyro[1] - GYRO_Y_OFFSET) * GYRO_GAIN_Y);
-    gyro_v[2] = DEG2RAD((gyro[2] - GYRO_Z_OFFSET) * GYRO_GAIN_Z);
+    gyro_v[2] = DEG2RAD((gyro[2] - GYRO_Z_OFFSET) * GYRO_GAIN_Z);*/
 }
 
 void GY80::Read_Magn(float* magn_v)
@@ -113,15 +117,15 @@
     Wire.write(MAGN_ADDRESS, buff, 1);
 
     // Request 6 bytes
-    int mag[3];
+    //int mag[3];
     if (Wire.read(MAGN_ADDRESS, buff,6) == 0)  // All bytes received?
     {
-        mag[0] = (short) ((uint16_t) buff[1] << 8 | buff[0]);
-        mag[1] = (short) ((uint16_t) buff[3] << 8 | buff[2]);
-        mag[2] = (short) ((uint16_t) buff[5] << 8 | buff[4]);
+        magn_v[0] = (short) ((uint16_t) buff[1] << 8 | buff[0]);
+        magn_v[1] = (short) ((uint16_t) buff[3] << 8 | buff[2]);
+        magn_v[2] = (short) ((uint16_t) buff[5] << 8 | buff[4]);
     }
-    magn_v[0] = (mag[0] - MAGN_X_OFFSET) * MAGN_X_SCALE;
+    /*magn_v[0] = (mag[0] - MAGN_X_OFFSET) * MAGN_X_SCALE;
     magn_v[1] = (mag[1] - MAGN_Y_OFFSET) * MAGN_Y_SCALE;
-    magn_v[2] = (mag[2] - MAGN_Z_OFFSET) * MAGN_Z_SCALE;
+    magn_v[2] = (mag[2] - MAGN_Z_OFFSET) * MAGN_Z_SCALE; */
 }
 

--- a/main.cpp	Tue Aug 26 06:46:55 2014 +0000
+++ b/main.cpp	Thu Aug 28 07:50:06 2014 +0000
@@ -82,7 +82,7 @@
         pc.printf("\n \r Wait for new connection...\n");                // pc info
         TCPSocketConnection client;                                     // TCP/IP client object
         server.accept(client);                                          // accept connection
-        client.set_blocking(false, 1500);                               // no blocking connections, timeout after (1.5)s
+        client.set_blocking(false, 4000);                               // no blocking connections, timeout after (1.5)s
         pc.printf("\n \r Connection from: %s\n", client.get_address()); // pc info
         char buffer[256];                                               // Data buffer
         while (true) {                                                  // loop
@@ -110,6 +110,9 @@
     sensor.Read_Accel(accel);                                           // read acceleration
     sensor.Read_Gyro(gyro);                                             // read gyroscope
     TSI_pos = tsi.readPercentage();                                     // read percentage position of touch sensor
+    //double pitch = (atan2(-accel[1], accel[2]) * 180.0) / 3.14;  
+    //double roll = (atan2(accel[0], sqrt(accel[1]*[accel[1] + accel[2] * accel[2])) * 180.0) / 3.14;
+    //pc.printf("roll: %f pitch: %f ,roll", pitch);
 }
 
 void comm_handler(char* command)
@@ -120,13 +123,13 @@
                                                                         
     pc.printf("\n \r %s \n \r", command);                               // send received command to pc (DEBUG)
     if(strcmp(command,"read_sensor")==0) {                              // data read from sensor
-       sprintf(command, "{""magnX"":%10f,""magnY"":%10f,""magnZ"":%10f,""accelX"":%10f,""accelY"":%10f,""accelZ"":%10f,""gyroX"":%10f,""gyroY"":%10f,""gyroZ"":%10f,""TSIpos"":%10f}", magn[0], magn[1], magn[2], accel[0], accel[1], accel[2], gyro[0], gyro[1], gyro[2], TSI_pos);         // enter data to buffer
+       sprintf(command, "{\"magnX\":%10f,\"magnY\":%10f,\"magnZ\":%10f,\"accelX\":%10f,\"accelY\":%10f,\"accelZ\":%10f,\"gyroX\":%10f,\"gyroY\":%10f,\"gyroZ\":%10f,\"TSIpos\":%10f}", magn[0], magn[1], magn[2], accel[0], accel[1], accel[2], gyro[0], gyro[1], gyro[2], TSI_pos);         // enter data to buffer
         wait(0.001);                                                    // wait
         pc.printf("\n \r sending sensor data! %s", command);            // pc info (debug)
     } else if (strcmp(command,"check_ports")==0) {                      // check I/O ports current values 
-        sprintf(command, "{""P1"":%d,""P2"":%d,""P3"":%d,""P4"":%d,""P5"":%d,""P6"":%d}", P1.read(), P2.read(), P3.read(), P4.read(), P5.read(), P6.read());
+        sprintf(command, "{\"P1\":%d,\"P2\":%d,\"P3\":%d,\"P4\":%d,\"P5\":%d,\"P6\":%d}", P1.read(), P2.read(), P3.read(), P4.read(), P5.read(), P6.read());
     } else if (strcmp(command,"get_P1")==0) {                           // get P1 value
-        sprintf(command, "{P1:%d}", P1.read());                         //
+        sprintf(command, "{\"P1\":%d}", P1.read());                         //
     } else if(strcmp(command,"set_P1_o")==0) {                          // set P1 as output
         P1.output();                                                    //
         sprintf(command, "P1 is now set as output");                    //
@@ -136,13 +139,13 @@
     } else if (strcmp(command,"set_P1_1")==0) {                         // set P1 as 1
         P1.output();                                                    // set to output
         P1.write(1);                                                    // write 1 to P1
-        sprintf(command, "{P1:%d}", P1.read());                         // reply with current value to check
+        sprintf(command, "{\"P1\":%d}", P1.read());                         // reply with current value to check
     } else if (strcmp(command,"set_P1_0")==0) {                         // set P1 as 0
         P1.output();                                                    //
         P1.write(0);
-        sprintf(command, "{P1:%d}", P1.read());
+        sprintf(command, "{\"P1\":%d}", P1.read());
     } else if (strcmp(command,"get_P2")==0) {
-        sprintf(command, "P2 value: %d", P2.read());
+        sprintf(command, "{\"P2\":%d}", P2.read());
     } else if (strcmp(command,"set_P2_o")==0) {
         P2.output();
         sprintf(command, "P2 is now set as output");
@@ -156,9 +159,9 @@
     } else if (strcmp(command,"set_P2_0")==0) {
         P2.output();
         P2.write(0);
-        sprintf(command, "P2 value is now: %d ", P2.read());
+        sprintf(command, "{\"P2\":%d}", P2.read());
     } else if (strcmp(command,"get_P3")==0) {
-        sprintf(command, "P3 value: %d", P3.read());
+        sprintf(command, "{\"P3\":%d}", P3.read());
     } else if (strcmp(command,"set_P3_o")==0) {
         P3.output();
         sprintf(command, "P3 is now set as output");
@@ -168,13 +171,13 @@
     } else if (strcmp(command,"set_P3_1")==0) {
         P3.output();
         P3.write(1);
-        sprintf(command, "P3 value is now: %d ", P3.read());
+        sprintf(command, "{\"P3\":%d}", P3.read());
     } else if (strcmp(command,"set_P3_0")==0) {
         P3.output();
         P3.write(0);
         sprintf(command, "P3 value is now: %d ", P3.read());
     } else if (strcmp(command,"get_P4")==0) {
-        sprintf(command, "P4 value: %d", P4.read());
+        sprintf(command, "{\"P4\":%d}", P4.read());
     } else if (strcmp(command,"set_P4_o")==0) {
         P4.output();
         sprintf(command, "P4 is now set as output");
@@ -184,13 +187,13 @@
     } else if (strcmp(command,"set_P4_1")==0) {
         P4.output();
         P4.write(1);
-        sprintf(command, "P4 value is now: %d ", P4.read());
+        sprintf(command, "{\"P4\":%d}", P4.read());
     } else if (strcmp(command,"set_P4_0")==0) {
         P4.output();
         P4.write(0);
-        sprintf(command, "P4 value is now: %d ", P4.read());
+        sprintf(command, "{\"P4\":%d}", P4.read());
     } else if (strcmp(command,"get_P5")==0) {
-        sprintf(command, "P5 value: %d", P5.read());
+        sprintf(command, "{\"P5:\"%d}", P5.read());
     } else if (strcmp(command,"set_P5_o")==0) {
         P5.output();
         sprintf(command, "P5 is now set as output");
@@ -200,13 +203,13 @@
     } else if (strcmp(command,"set_P5_1")==0) {
         P5.output();
         P5.write(1);
-        sprintf(command, "P5 value is now: %d ", P5.read());
+        sprintf(command, "{\"P5\":%d}", P5.read());
     } else if (strcmp(command,"set_P5_0")==0) {
         P5.output();
         P5.write(0);
-        sprintf(command, "P5 value is now: %d ", P5.read());
+        sprintf(command, "{\"P5\":%d}", P5.read());
     } else if (strcmp(command,"get_P6")==0) {
-        sprintf(command, "P6 value: %d", P6.read());
+        sprintf(command, "{\"P6\":%d}", P6.read());
     } else if (strcmp(command,"set_P6_o")==0) {
         P6.output();
         sprintf(command, "P6 is now set as output");
@@ -216,11 +219,11 @@
     } else if (strcmp(command,"set_P6_1")==0) {
         P6.output();
         P6.write(1);
-        sprintf(command, "P6 value is now: %d ", P6.read());
+        sprintf(command, "{\"P6\":%d}", P6.read());
     } else if (strcmp(command,"set_P6_0")==0) {
         P6.output();
         P6.write(0);
-        sprintf(command, "P6 value is now: %d ", P6.read());
+        sprintf(command, "{\"P6\":%d}", P6.read());
     } else sprintf(command, "Unknown Command!");
 }