Department of Electrical Eng University of Moratuwa / Mbed 2 deprecated Nucleo-F411RE-Servo-Test_V2

a program with MD25

Dependencies:   BufferedSerial MD25 Servo mbed

main.cpp@0:7e2f1e96c3d9, 2017-01-26 (annotated)

Committer:
eembed
Date:
Thu Jan 26 03:10:36 2017 +0000
Revision:
0:7e2f1e96c3d9
a program with MD25

Who changed what in which revision?

UserRevisionLine numberNew contents of line
eembed 0:7e2f1e96c3d9 1 #include "mbed.h"
eembed 0:7e2f1e96c3d9 2 #include "MD25.h"
eembed 0:7e2f1e96c3d9 3 #include "Servo.h"
eembed 0:7e2f1e96c3d9 4 #include "math.h"
eembed 0:7e2f1e96c3d9 5 #include "BufferedSerial.h"
eembed 0:7e2f1e96c3d9 6
eembed 0:7e2f1e96c3d9 7 Serial pc (USBTX, USBRX);
eembed 0:7e2f1e96c3d9 8 BufferedSerial cam1 (PA_9, PA_10, 256, 4);
eembed 0:7e2f1e96c3d9 9 BufferedSerial cam2 (PC_6, PC_7, 256, 4);
eembed 0:7e2f1e96c3d9 10
eembed 0:7e2f1e96c3d9 11 MD25 Motor_Comm (PB_9,PB_8); //Define serial comms
eembed 0:7e2f1e96c3d9 12 Servo Servo1 (PA_8); //Define Servo Pin
eembed 0:7e2f1e96c3d9 13 Servo Servo2 (PB_10);
eembed 0:7e2f1e96c3d9 14
eembed 0:7e2f1e96c3d9 15 //Global Variables
eembed 0:7e2f1e96c3d9 16 int32_t Enc1_cnt = 0;
eembed 0:7e2f1e96c3d9 17 float speed1 = 0.0;
eembed 0:7e2f1e96c3d9 18 float speed2 = 0.0;
eembed 0:7e2f1e96c3d9 19
eembed 0:7e2f1e96c3d9 20 int readFromSerial (BufferedSerial& serial)
eembed 0:7e2f1e96c3d9 21 {
eembed 0:7e2f1e96c3d9 22 int value = 0;
eembed 0:7e2f1e96c3d9 23 char temp = 0;
eembed 0:7e2f1e96c3d9 24 // pc.printf("hi \n");
eembed 0:7e2f1e96c3d9 25 while (serial.readable())
eembed 0:7e2f1e96c3d9 26 {
eembed 0:7e2f1e96c3d9 27 temp = serial.getc();
eembed 0:7e2f1e96c3d9 28 pc.printf("temp = %d\n", temp); //-------------
eembed 0:7e2f1e96c3d9 29 // if (temp == '\n') break;
eembed 0:7e2f1e96c3d9 30 // else value = value * 10 + (temp - '0');
eembed 0:7e2f1e96c3d9 31 // pc.printf("value = %d\n",value);
eembed 0:7e2f1e96c3d9 32
eembed 0:7e2f1e96c3d9 33 }
eembed 0:7e2f1e96c3d9 34 //pc.printf("value = %d\n",value);
eembed 0:7e2f1e96c3d9 35 return temp;
eembed 0:7e2f1e96c3d9 36 }
eembed 0:7e2f1e96c3d9 37
eembed 0:7e2f1e96c3d9 38 void initializeMotor (MD25& motor)
eembed 0:7e2f1e96c3d9 39 {
eembed 0:7e2f1e96c3d9 40 motor.setMode(1);
eembed 0:7e2f1e96c3d9 41 motor.setCommand(0x31); //Enables automatic speed regulation
eembed 0:7e2f1e96c3d9 42 motor.setCommand(0x20); //Resets the encoder registers to zero
eembed 0:7e2f1e96c3d9 43 motor.setCommand(0x32); //Disables 2 second timeout of motors
eembed 0:7e2f1e96c3d9 44 motor.setSpeedRegisters(0,0); //Motor Start Speed Off
eembed 0:7e2f1e96c3d9 45 }
eembed 0:7e2f1e96c3d9 46
eembed 0:7e2f1e96c3d9 47 int main()
eembed 0:7e2f1e96c3d9 48 {
eembed 0:7e2f1e96c3d9 49 float angle1 = 1.0; //Start Angle
eembed 0:7e2f1e96c3d9 50 float angle2 = 1.0; //Start Angle
eembed 0:7e2f1e96c3d9 51 cam1.baud(9600);
eembed 0:7e2f1e96c3d9 52 cam2.baud(9600);
eembed 0:7e2f1e96c3d9 53
eembed 0:7e2f1e96c3d9 54 //pc.printf("Encoder count = %d\n", Enc1_cnt);
eembed 0:7e2f1e96c3d9 55 initializeMotor (Motor_Comm);
eembed 0:7e2f1e96c3d9 56
eembed 0:7e2f1e96c3d9 57 float range = 0.00075;
eembed 0:7e2f1e96c3d9 58 Servo1.calibrate(range, 30.0);
eembed 0:7e2f1e96c3d9 59 Servo1.write(angle1);
eembed 0:7e2f1e96c3d9 60 Servo2.calibrate(range, 30.0);
eembed 0:7e2f1e96c3d9 61 Servo2.write(angle2);
eembed 0:7e2f1e96c3d9 62
eembed 0:7e2f1e96c3d9 63 wait(1);
eembed 0:7e2f1e96c3d9 64
eembed 0:7e2f1e96c3d9 65 while(1)
eembed 0:7e2f1e96c3d9 66 {
eembed 0:7e2f1e96c3d9 67 while (angle1 >= 0 && angle2 >= 0)
eembed 0:7e2f1e96c3d9 68 {
eembed 0:7e2f1e96c3d9 69 angle1 = readFromSerial(cam1);
eembed 0:7e2f1e96c3d9 70 angle2 = readFromSerial(cam2);
eembed 0:7e2f1e96c3d9 71 pc.printf("A1 = %f\n", angle1);
eembed 0:7e2f1e96c3d9 72 pc.printf("A2 = %f\n", angle2);
eembed 0:7e2f1e96c3d9 73 Servo1.write(angle1);
eembed 0:7e2f1e96c3d9 74 Servo2.write(angle2);
eembed 0:7e2f1e96c3d9 75 speed1 = 50 / (cos(angle1)); // Division by 0 error?
eembed 0:7e2f1e96c3d9 76 speed2 = 50 / (cos(angle2)); // Division by 0 error?
eembed 0:7e2f1e96c3d9 77 Motor_Comm.setSpeedRegisters(speed1, speed2);
eembed 0:7e2f1e96c3d9 78 //Enc1_cnt = Motor_Comm.getEncoder1();
eembed 0:7e2f1e96c3d9 79 //pc.printf("Encoder count = %d\n", Enc1_cnt);
eembed 0:7e2f1e96c3d9 80 // angle1 = angle1 - 0.05;
eembed 0:7e2f1e96c3d9 81 // angle2 = angle2 - 0.05;
eembed 0:7e2f1e96c3d9 82 wait(1.5);
eembed 0:7e2f1e96c3d9 83 }
eembed 0:7e2f1e96c3d9 84 Motor_Comm.stopMotors();
eembed 0:7e2f1e96c3d9 85 }
eembed 0:7e2f1e96c3d9 86 }