File content as of revision 0:7e2f1e96c3d9:

#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(); 
    }
}