Mateusz Jaskula
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!");
}