Diff: main.cpp

Revision:

0:7e2f1e96c3d9

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Jan 26 03:10:36 2017 +0000
@@ -0,0 +1,86 @@
+#include "mbed.h"
+#include "MD25.h"
+#include "Servo.h"
+#include "math.h"
+#include "BufferedSerial.h"
+ 
+Serial pc (USBTX, USBRX);
+BufferedSerial cam1 (PA_9, PA_10, 256, 4);
+BufferedSerial cam2 (PC_6, PC_7, 256, 4);
+ 
+MD25 Motor_Comm (PB_9,PB_8);     //Define serial comms
+Servo Servo1 (PA_8);             //Define Servo Pin
+Servo Servo2 (PB_10);
+
+//Global Variables
+int32_t Enc1_cnt = 0;
+float speed1 = 0.0;
+float speed2 = 0.0;
+
+int readFromSerial (BufferedSerial& serial)
+{
+    int value = 0;
+    char temp = 0;
+  //  pc.printf("hi \n");
+    while (serial.readable())
+    {        
+        temp = serial.getc();
+        pc.printf("temp = %d\n", temp); //-------------
+       // if (temp == '\n') break;
+   //     else value = value * 10 + (temp - '0');
+   //     pc.printf("value = %d\n",value);
+        
+    }
+    //pc.printf("value = %d\n",value);
+    return temp;
+}
+
+void initializeMotor (MD25& motor)
+{
+    motor.setMode(1);
+    motor.setCommand(0x31);    //Enables automatic speed regulation
+    motor.setCommand(0x20);    //Resets the encoder registers to zero
+    motor.setCommand(0x32);    //Disables 2 second timeout of motors
+    motor.setSpeedRegisters(0,0);  //Motor Start Speed Off
+}
+
+int main()
+{
+    float angle1 = 1.0;           //Start Angle
+    float angle2 = 1.0;           //Start Angle
+    cam1.baud(9600);
+    cam2.baud(9600);
+    
+    //pc.printf("Encoder count = %d\n", Enc1_cnt);
+    initializeMotor (Motor_Comm);
+    
+    float range = 0.00075;
+    Servo1.calibrate(range, 30.0);
+    Servo1.write(angle1);
+    Servo2.calibrate(range, 30.0);
+    Servo2.write(angle2);
+    
+    wait(1);
+    
+    while(1)
+    {
+        while (angle1 >= 0 && angle2 >= 0)
+        {
+            angle1 = readFromSerial(cam1);
+            angle2 = readFromSerial(cam2);
+            pc.printf("A1 = %f\n", angle1); 
+            pc.printf("A2 = %f\n", angle2); 
+            Servo1.write(angle1);
+            Servo2.write(angle2);
+            speed1 = 50 / (cos(angle1));  // Division by 0 error?
+            speed2 = 50 / (cos(angle2));  // Division by 0 error?
+            Motor_Comm.setSpeedRegisters(speed1, speed2);
+            //Enc1_cnt = Motor_Comm.getEncoder1();
+            //pc.printf("Encoder count = %d\n", Enc1_cnt);
+         //   angle1 = angle1 - 0.05;
+         //   angle2 = angle2 - 0.05;
+            wait(1.5);
+        }
+        Motor_Comm.stopMotors(); 
+    }
+}
\ No newline at end of file