Diff: main.cpp

Revision:

7:1f4ef899e43e

Parent:

6:beddcb25ad4e

diff -r beddcb25ad4e -r 1f4ef899e43e main.cpp
--- a/main.cpp	Mon Sep 16 12:06:13 2019 +0000
+++ b/main.cpp	Fri Sep 20 03:53:31 2019 +0000
@@ -67,7 +67,6 @@
 #endif
 
 #ifdef BUTTON_MODE
-
 void rxData(){
     char temp = bt1.getc();
     bt1.putc(0);            //convention
@@ -76,7 +75,6 @@
         rxFlag = 1;
         }
 }
-
 #endif
 
 #ifdef JOYSTICK_MODE
@@ -84,15 +82,34 @@
 int Y_position = 0;
 
 void rxData(){
-    char temp[20];
-    temp = bt1.gets();
-    bt1.printf(0);            //convention
-//    if(temp== '1'||temp== '2'||temp== '3'||temp== '4'||temp== '5'||temp== '6'||temp=='7'){
-//        rxChar = temp;
-//        rxFlag = 1;
-//        }
+    char temp[2] = {32,32};
+    if(bt1.readable()){
+    for(int i=0; i<2; i++){
+        temp[i] = bt1.getc();
+    }
+        X_position = temp[1];
+        Y_position = temp[0];
+    }
 }
 #endif
+float map(int in, float inMin, float inMax, float outMin, float outMax) {
+  // check it's within the range
+  if (inMin<inMax) { 
+    if (in <= inMin) 
+      return outMin;
+    if (in >= inMax)
+      return outMax;
+  } else {  // cope with input range being backwards.
+    if (in >= inMin) 
+      return outMin;
+    if (in <= inMax)
+      return outMax;
+  }
+  // calculate how far into the range we are
+  float scale = (in-inMin)/(inMax-inMin);
+  // calculate the output.
+  return outMin + scale*(outMax-outMin);
+}
 int main()
 {
     throttleL = 0;
@@ -164,9 +181,63 @@
         bt2.printf("%d|%d|%d\n", dis1, dis2, dis3);
         
         #ifdef JOYSTICK_MODE        
-        
+
+        if (Y_position < 32) {
+    // Convert the declining Y-axis readings for going backward from 470 to 0 into 0 to 255 value for the PWM signal for increasing the motor speed
+                        reverseR = 1;
+                        reverseL = 1;
+    throttleR = map(Y_position, 32, 0, 0, 1);
+    throttleL = map(Y_position, 32, 0, 0, 1);
+        }
+        else if (Y_position > 32) {
+    // Convert the increasing Y-axis readings for going forward from 550 to 1023 into 0 to 255 value for the PWM signal for increasing the motor speed
+                        reverseR = 0;
+                        reverseL = 0;
+        throttleR = map(Y_position, 32, 64, 0, 1);
+        throttleL = map(Y_position, 32, 64, 0, 1);
+        }
+  // If joystick stays in middle the motors are not moving
+        else {
+        throttleR = 0;
+        throttleL = 0;
+        }
         
+        // X-axis used for left and right control
+        if (X_position < 32) {
+    // Convert the declining X-axis readings from 470 to 0 into increasing 0 to 255 value
+        int xMapped = map(X_position, 32, 0, 0, 0.3);
+    // Move to left - decrease left motor speed, increase right motor speed
+        throttleL = throttleL - xMapped;
+        throttleR = throttleR + xMapped;
+    // Confine the range from 0 to 255
+        if (throttleL < 0) {
+        throttleL = 0;
+        }
+        if (throttleR > 255) {
+        throttleR = 255;
+        }
+        }
+        if (X_position > 32) {
+    // Convert the increasing X-axis readings from 550 to 1023 into 0 to 255 value
+        int xMapped = map(X_position, 32, 64, 0, 0.3);
+    // Move right - decrease right motor speed, increase left motor speed
+        throttleL = throttleL + xMapped;
+        throttleR = throttleR - xMapped;
+    // Confine the range from 0 to 255
+        if (throttleL > 255) {
+        throttleL = 255;
+        }
+        if (throttleR < 0) {
+        throttleR = 0;
+        }
+        }
         
+        if (throttleR < 0.4) {
+        throttleR = 0;
+        }
+        if (throttleL < 0.4) {
+        throttleL = 0;
+        }
         #endif
         
         #ifdef BUTTON_MODE