robot moves turns

controls/platformio.ini

@@ -12,4 +12,7 @@
 platform = atmelavr
 board = uno
 framework = arduino
-lib_deps = featherfly/SoftwareSerial@^1.0
+lib_deps = 
+	featherfly/SoftwareSerial@^1.0
+	eecharlie/MatrixMath@^1.0
+	arduino-libraries/Servo@^1.1.8

controls/src/main.cpp

@@ -1,112 +1,81 @@
 #include <Arduino.h>
 #include <softwareserial.h> // remove the inverted commas after you copy the code to the IDE
 #include "motor_control.h"
-#include <string> 
+#include <string.h> 
+#include <Servo.h>
 
 SoftwareSerial BT(10, 11); 
+Servo servo;  // create servo object to control a servo
+// twelve servo objects can be created on most boards
+
+int pos = 180;
+
 // creates a "virtual" serial port/UART
 // connect BT module TX to D10
 // connect BT module RX to D11
 // connect BT Vcc to 5V, GND to GND
-String value= "";
+
+String value = "";
 void setup(){
+  servo.attach(3);
+  BT.begin(9600);
   Serial.begin(9600);
   motor_init();
+  BT.println("Hello from Arduino");
+  servo.write(180);
 }
-
+ 
 void loop(){
-
-  if(Serial.available() > 0){
-    value = Serial.readStringUntil('#');
-  }
-  if(value.length() == 7){
-    String angle1 = value.substring(0,3);
-    String strength1 = value.substring(3,6);
-    String button1 = value.substring(6,8);
-    double angle2 = stod(angle1);
-    double strength2 = stod(strength1);
-    double button2 = stod(button1);
-    Serial.print("angle;");
-    move_bot(0,double(strength2), double(angle2));
-    Serial.print(angle2);
-    Serial.print('\t');
-    Serial.print("strength;");
-    Serial.print(strength2);
-    Serial.print('\t');
-    Serial.print("button;");
-    Serial.print(button2);
-    Serial.println("");
-    Serial.flush();
-    value = "";
+  if(BT.available() > 0){
+    value = BT.readStringUntil('#');
+    //Serial.println(value);
+    if(value.length() == 7){
+      String angle1 = value.substring(0,3);
+      String strength1 = value.substring(3,6);
+      String button = value.substring(6, 8);
+      double angle2 = angle1.toDouble();
+      double strength2 = strength1.toDouble();
+      double button2 = button.toDouble();
+      if ((angle2<=90) | (angle2>=270)){
+        if (angle2<=90){
+          move_bot(strength2/100, 0, -angle2);
+        }
+        else {
+          move_bot(strength2/100, 0, angle2-270);
+        }
+      }
+      else {
+        if (angle2<=180){
+          move_bot(-strength2/100, 0, -angle2);
+        }
+        else {
+          move_bot(-strength2/100, 0, angle2-180);
+        }
+      }
+      if (button2 == 1){
+        move_bot(0,0,0);
+        BT.flush();
+      }
+      else if (button2 == 4) {
+        if (pos==180){
+          pos=0;
+          servo.write(pos);
+        }
+        else {
+          pos=180;
+          servo.write(pos);
+        }
+      }
+      Serial.print("angle;");
+      Serial.print(angle2);
+      Serial.print('\t');
+      Serial.print("strength;");
+      Serial.print(strength2);
+      Serial.print('\t');
+      Serial.println("");
+      Serial.flush();
+      value = "";
+    }
   }
 }
 
-
-
-
-
-
-
-
-
-
-
-// void setup()  
-// {
-//   // set digital pin to control as an output
-//   pinMode(13, OUTPUT);
-//   // set the data rate for the SoftwareSerial port
-//   BT.begin(9600);
-//   // Send test message to other device
-//   BT.println("Hello from Arduino");
-// }
-// char a; // stores incoming character from other device
-// void loop() 
-// {
-//   if (BT.available())// if text arrived in from BT serial...
-//   {
-//     a=(BT.read());
-//     if (a=='1')
-//     {
-//       digitalWrite(13, HIGH);
-//       BT.println("LED on");
-//     }
-//     if (a=='2')
-//     {
-//       digitalWrite(13, LOW);
-//       BT.println("LED off");
-//     }
-//     if (a=='?')
-//     {
-//       BT.println("Send '1' to turn LED on");
-//       BT.println("Send '2' to turn LED on");
-//     }   
-
-//     if(a == 'N'){
-
-//     }
-//     if(a == 'E'){
-
-//     }
-//     if(a == 'S'){
-
-//     }
-//     if(a == 'W'){
-
-//     }
-//     if(a == 'A'){
-
-//     }
-//     if(a == 'B'){
-
-//     }
-//     if(a == 'C'){
-
-//     }
-//     if(a == 'D'){
-
-//     }
-
-//     // you can add more "if" statements with other characters to add more commands
-//   }
-

controls/src/motor_control.cpp

@@ -0,0 +1,55 @@
+#include <Arduino.h> 
+#include <MatrixMath.h>
+
+#define M1A 6
+#define M1B 7
+#define M2A 4
+#define M2B 5
+#define M3A 8
+#define M3B 9
+
+mtx_type gauss[3][3] = {{0.58, -0.33, 0.33}, {-0.58, -0.33, 0.33}, {0, 0.67, 0.33}};
+//mtx_type gauss[3][3] = {{0, -0.586, 0.414}, {-0.707, -0.293, 0.293}, {0.707, 0.293, 0.293}};
+mtx_type inp[3];
+mtx_type output[3]; 
+double f1, f2, f3 = 0; 
+
+void motor_init()
+{
+    // Set the output pins
+    pinMode(M1A, OUTPUT);
+    pinMode(M1B, OUTPUT);
+    pinMode(M2A, OUTPUT);
+    pinMode(M2B, OUTPUT);
+    pinMode(M3A, OUTPUT);
+    pinMode(M3B, OUTPUT);
+}
+
+//determine how to move each motor 
+void motor_control(int A, int B, double acc){
+    if (acc<0){
+        analogWrite(A, 0);
+        analogWrite(B, -(acc*255));
+    }
+    else if (acc==0){
+        digitalWrite(A, HIGH);
+        digitalWrite(B, HIGH);
+    }
+    else {
+        analogWrite(A, (acc*255));
+        analogWrite(B, 0);
+    }
+}
+//determine how to move the entire robot 
+void move_bot(double ax, double ay, int w){
+  inp[0]=ax;
+  inp[1]=ay;
+  inp[2]=w; 
+  Matrix.Multiply((mtx_type*) gauss,(mtx_type*) inp, 3 , 3 , 1, (mtx_type*) output);
+  f1 = output[0];
+  f2 = output[1];
+  f3 = output[2];
+  motor_control(M1A, M1B, f1);
+  motor_control(M2A, M2B, f2);
+  motor_control(M3A, M3B, f3);
+}

motor_control/src/main.cpp

@@ -1,12 +1,12 @@
 #include <Arduino.h> 
 #include <MatrixMath.h>
 
-#define M1A 4
-#define M1B 5
-#define M2A 6
-#define M2B 7
-#define M3A 8
-#define M3B 9
+#define M1A 8
+#define M1B 9
+#define M2A 4
+#define M2B 5
+#define M3A 6
+#define M3B 7
 
 mtx_type gauss[3][3] = {{0.58, -0.33, 0.33}, {-0.58, -0.33, 0.33}, {0, 0.67, 0.33}};
 mtx_type inp[3];