Mateusz Jaskula
/
KL25Z_TangoIO
Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
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
| GY80/GY80.cpp | Show annotated file Show diff for this revision Revisions of this file |
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- 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!");
}