Mateusz Jaskula
/
KL25Z_TangoIO
KL25 driver for Tango Control System
Diff: main.cpp
- Revision:
- 2:9fe6f1e273b4
- Parent:
- 1:f1ee978773d3
--- 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!"); }