a program with MD25
Dependencies: BufferedSerial MD25 Servo mbed
Revision 0:7e2f1e96c3d9, committed 2017-01-26
- Comitter:
- eembed
- Date:
- Thu Jan 26 03:10:36 2017 +0000
- Commit message:
- a program with MD25
Changed in this revision
diff -r 000000000000 -r 7e2f1e96c3d9 BufferedSerial.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/BufferedSerial.lib Thu Jan 26 03:10:36 2017 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/sam_grove/code/BufferedSerial/#a0d37088b405
diff -r 000000000000 -r 7e2f1e96c3d9 MD25.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/MD25.lib Thu Jan 26 03:10:36 2017 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/jimherd/code/MD25/#e575d390c730
diff -r 000000000000 -r 7e2f1e96c3d9 Servo.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Servo.lib Thu Jan 26 03:10:36 2017 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/simon/code/Servo/#36b69a7ced07
diff -r 000000000000 -r 7e2f1e96c3d9 main.cpp --- /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
diff -r 000000000000 -r 7e2f1e96c3d9 mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Thu Jan 26 03:10:36 2017 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/0ab6a29f35bf \ No newline at end of file