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?
User | Revision | Line number | New 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 | } |