Adding comms on jetson

Author: wizardofrobots

beachRover-v1-arduino/Communication.cpp

@@ -21,6 +21,22 @@ long readLong() {
   return NULL;
 }
 
+float readFloat(){
+  unsigned long startTime = millis();
+  while((Serial.available()<sizeof(float) && (millis() - startTime)<timeout)){};
+  if(Serial.available()>0){
+    size_t bytesRead;
+    byte floatBuffer[sizeof(float)];
+    bytesRead = Serial.readBytes(floatBuffer,sizeof(float));
+    if(bytesRead==sizeof(float)){
+      float receivedFloat;
+      memcpy(&receivedFloat,floatBuffer,sizeof(float));
+      return receivedFloat;
+    }
+  }
+  return NULL;
+}
+
 int readInt() {
   unsigned long startTime = millis();
   while ((Serial.available() < sizeof(int)) && (millis() - startTime) < timeout) {};
@@ -66,7 +82,8 @@ bool readCommand(Command* command) {
       if (commandType == NULL) {
         return false;
       }
-      long commandParam = readLong();
+      float commandParam = readFloat();
+//      Serial.println(commandParam);
       if (commandParam == NULL) {
         return false;
       }
@@ -82,3 +99,21 @@ bool readCommand(Command* command) {
   }
   return false;
 }
+
+void sendInfo(Info * info){
+  if(Serial.availableForWrite()){
+//    Serial.println('^');
+//    Serial.println(info->sensor);
+//    Serial.println(info->x);
+//    Serial.println(info->y);
+//    Serial.println(info->z);
+
+    
+    Serial.write('^');
+    Serial.write((byte *)&(info->sensor),2);
+    Serial.write((byte *)&(info->x),4);
+    Serial.write((byte *)&(info->y),4);
+    Serial.write((byte *)&(info->z),4);
+    Serial.write(10);
+  }
+}

beachRover-v1-arduino/Communication.h

@@ -12,17 +12,19 @@ enum COMMAND_TYPE{
 
 struct Info{
   SENSOR_TYPE sensor;
-  int x;
-  int y;
-  int z;
+  float x;
+  float y;
+  float z;
 };
 
 struct Command{
   COMMAND_TYPE command;
-  long param;
+  float param;
 };
 
 int readInt();
 long readLong();
+float readFloat();
 double readDouble();
 bool readCommand(Command* commander);
+void sendInfo(Info* info);

beachRover-v1-arduino/beachRover-v1-arduino.ino

@@ -83,37 +83,44 @@ void loop() {
 
   int x, y, z;
   compass.read(&x, &y, &z);
-  float heading = atan2(y, x);
 
-  float headingDegrees = heading * 180 / M_PI;
-  float target = 0.;
-  float error = target - headingDegrees;
-
-  if (abs(error) > 180 && error > 0) {
-    error = 180 - error;
-  } else if (abs(error) > 180 && error < 0) {
-    error = (int)(180 - error) % 360;
-  }
-
-  float correction = error;
-
-  if (error >= 0) {
-    indicate_led(YELLOW_LED, false);
-  } else {
-    indicate_led(YELLOW_LED, true);
-  }
-
-  Command receivedCommand;
-  bool commandReceived = readCommand(&receivedCommand);
-  if (commandReceived) {
-    Serial.println(receivedCommand.command);
-    Serial.println(receivedCommand.param);
-  }
-
-
-  motorDirection = kalman(motorDirection, (correction / 360) + 0.5, 0.98);
-  motorDirection = (correction / 360) + 0.5;
-  driveMotor(motorDirection, 0.35);
+  Info info;
+  info.sensor = MAGNETOMETER;
+  info.x = (float)x;
+  info.y = (float)y;
+  info.z = (float)z;
+//  Serial.println(info.x);
+  sendInfo(&info);
+//  float heading = atan2(y, x);
+//
+//  float headingDegrees = heading * 180 / M_PI;
+//  float target = 0.;
+//  float error = target - headingDegrees;
+//
+//  if (abs(error) > 180 && error > 0) {
+//    error = 180 - error;
+//  } else if (abs(error) > 180 && error < 0) {
+//    error = (int)(180 - error) % 360;
+//  }
+//
+//  float correction = error;
+
+//  Command receivedCommand;
+//  bool commandReceived = readCommand(&receivedCommand);
+//  if (commandReceived) {
+////    Serial.println(receivedCommand.command);
+////    Serial.println(receivedCommand.param);
+//    if (receivedCommand.param >= 0) {
+//      indicate_led(YELLOW_LED, false);
+//    } else {
+//      indicate_led(YELLOW_LED, true);
+//    }
+//  }
+//
+
+//  motorDirection = kalman(motorDirection, (correction / 360) + 0.5, 0.98);
+//  motorDirection = (correction / 360) + 0.5;
+//  driveMotor(motorDirection, 0.35);
   delay(10);
 }