Code Controlling Robot DC Motors and Crane Servos
Dependencies: mbed Servo Motordriver
main.cpp
- Committer:
- ohodge3
- Date:
- 2020-04-29
- Revision:
- 0:316cfb89e9cd
File content as of revision 0:316cfb89e9cd:
// full-speed reverse (-1.0) // full speed forwards (1.0) #include "mbed.h" #include "motordriver.h" #include "Servo.h" Motor left(p22, p12, p11, 1); // pwm, fwd, rev (Motor serialChar) Motor right(p21, p10, p9, 1); // pwm, fwd, rev (Motor B) Servo shoulder(p23); Servo elbow(p24); Servo wrist(p26); Servo wrist(p25); DigitalOut led1(LED1); DigitalOut led2(LED2); DigitalOut led3(LED3); DigitalOut led4(LED4); //set up xbee Serial xbee(p13, p14);//TX not used Serial pc(USBTX, USBRX); DigitalOut rst(p7); //speed variables for motors volatile float speed1; volatile float speed2; void go(){ left.speed(speed1); right.speed(speed2); } int main() { // reset xbee (at least 200ns) rst = 0; wait_ms(1); rst = 1; wait_ms(1); //initial speed is 0 speed1 = 0.0; speed2 = 0.0; //variables to hold serial characters for xbee char serialChar; char prev; //varaibles for arm servos volatile float shoulderNum = 0.5; volatile float elbowNum = 0.10; volatile float wristNum = 0.0; //loop while (1) { if(xbee.readable()){ prev = serialChar; serialChar = xbee.getc(); //left right rotation if (prev == 'a' && serialChar < 60){ shoulderNum = shoulderNum + 0.01; if (shoulderNum > 1) shoulderNum = 1; shoulder = shoulderNum; led1 = 1; } else if (prev == 'a' && serialChar > 110){ shoulderNum = shoulderNum - 0.01; if (shoulderNum < 0) shoulderNum = 0; shoulder = shoulderNum; led2 = 1; } else{ shoulder = shoulderNum; } //up down rotation if (prev == 'b' && serialChar < 60){ elbowNum = elbowNum + 0.01; if (elbowNum > 1) elbowNum = 1; elbow = elbowNum; led1 = 1; } else if (prev == 'b' && serialChar > 110){ elbowNum = elbowNum - 0.01; if (elbowNum < 0) elbowNum = 0; elbow = elbowNum; led2 = 1; } else{ elbow = elbowNum; } //motor rotation if (prev == 'c' && serialChar < 60){ speed1 = 1.0; speed2 = -1.0; go(); } else if (prev == 'c' && serialChar > 110){ speed1 = -1; speed2 = 1; go(); } else if (prev == 'c' && serialChar > 60 && serialChar < 110 && speed1 != speed2){ speed1 = 0; speed2 = 0; go(); } //motor forward and reverse if (prev == 'd' && serialChar < 60){ speed1 = -1.0; speed2 = -1.0; go(); } else if (prev == 'd' && serialChar > 110){ speed1 = 1; speed2 = 1; go(); } else if (prev == 'd' && serialChar > 60 && serialChar < 110 && speed1 == speed2){ speed1 = 0; speed2 = 0; go(); } //wrist controller if (prev == 'e' && serialChar == 36){ wristNum = wristNum + 0.01; if (wristNum > 1) wristNum = 1; wrist = wristNum; led1 = 1; } else if (prev == 'f' && serialChar == 36){ wristNum = wristNum - 0.01; if (wristNum < 0) wristNum = 0; wrist = wristNum; led2 = 1; } else{ wrist = wristNum; } //print to pc for debugging (Tera Term etc.) pc.putc(serialChar); } } }