Department of Electrical Eng University of Moratuwa
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
--- /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
--- /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
--- /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
--- /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
--- /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