ECE 4180 Project / Mbed 2 deprecated 4180_Crane_Robot

Code Controlling Robot DC Motors and Crane Servos

Dependencies:   mbed Servo Motordriver

main.cpp@0:316cfb89e9cd, 2020-04-29 (annotated)

Committer:
ohodge3
Date:
Wed Apr 29 20:02:35 2020 +0000
Revision:
0:316cfb89e9cd
Final Robot Code

Who changed what in which revision?

UserRevisionLine numberNew contents of line
ohodge3 0:316cfb89e9cd 1 // full-speed reverse (-1.0)
ohodge3 0:316cfb89e9cd 2 // full speed forwards (1.0)
ohodge3 0:316cfb89e9cd 3
ohodge3 0:316cfb89e9cd 4 #include "mbed.h"
ohodge3 0:316cfb89e9cd 5 #include "motordriver.h"
ohodge3 0:316cfb89e9cd 6 #include "Servo.h"
ohodge3 0:316cfb89e9cd 7
ohodge3 0:316cfb89e9cd 8 Motor left(p22, p12, p11, 1); // pwm, fwd, rev (Motor serialChar)
ohodge3 0:316cfb89e9cd 9 Motor right(p21, p10, p9, 1); // pwm, fwd, rev (Motor B)
ohodge3 0:316cfb89e9cd 10 Servo shoulder(p23);
ohodge3 0:316cfb89e9cd 11 Servo elbow(p24);
ohodge3 0:316cfb89e9cd 12 Servo wrist(p26);
ohodge3 0:316cfb89e9cd 13 Servo wrist(p25);
ohodge3 0:316cfb89e9cd 14 DigitalOut led1(LED1);
ohodge3 0:316cfb89e9cd 15 DigitalOut led2(LED2);
ohodge3 0:316cfb89e9cd 16 DigitalOut led3(LED3);
ohodge3 0:316cfb89e9cd 17 DigitalOut led4(LED4);
ohodge3 0:316cfb89e9cd 18
ohodge3 0:316cfb89e9cd 19
ohodge3 0:316cfb89e9cd 20 //set up xbee
ohodge3 0:316cfb89e9cd 21 Serial xbee(p13, p14);//TX not used
ohodge3 0:316cfb89e9cd 22 Serial pc(USBTX, USBRX);
ohodge3 0:316cfb89e9cd 23 DigitalOut rst(p7);
ohodge3 0:316cfb89e9cd 24
ohodge3 0:316cfb89e9cd 25 //speed variables for motors
ohodge3 0:316cfb89e9cd 26 volatile float speed1;
ohodge3 0:316cfb89e9cd 27 volatile float speed2;
ohodge3 0:316cfb89e9cd 28
ohodge3 0:316cfb89e9cd 29 void go(){
ohodge3 0:316cfb89e9cd 30 left.speed(speed1);
ohodge3 0:316cfb89e9cd 31 right.speed(speed2);
ohodge3 0:316cfb89e9cd 32 }
ohodge3 0:316cfb89e9cd 33
ohodge3 0:316cfb89e9cd 34 int main() {
ohodge3 0:316cfb89e9cd 35
ohodge3 0:316cfb89e9cd 36 // reset xbee (at least 200ns)
ohodge3 0:316cfb89e9cd 37 rst = 0;
ohodge3 0:316cfb89e9cd 38 wait_ms(1);
ohodge3 0:316cfb89e9cd 39 rst = 1;
ohodge3 0:316cfb89e9cd 40 wait_ms(1);
ohodge3 0:316cfb89e9cd 41
ohodge3 0:316cfb89e9cd 42 //initial speed is 0
ohodge3 0:316cfb89e9cd 43 speed1 = 0.0;
ohodge3 0:316cfb89e9cd 44 speed2 = 0.0;
ohodge3 0:316cfb89e9cd 45
ohodge3 0:316cfb89e9cd 46 //variables to hold serial characters for xbee
ohodge3 0:316cfb89e9cd 47 char serialChar;
ohodge3 0:316cfb89e9cd 48 char prev;
ohodge3 0:316cfb89e9cd 49
ohodge3 0:316cfb89e9cd 50 //varaibles for arm servos
ohodge3 0:316cfb89e9cd 51 volatile float shoulderNum = 0.5;
ohodge3 0:316cfb89e9cd 52 volatile float elbowNum = 0.10;
ohodge3 0:316cfb89e9cd 53 volatile float wristNum = 0.0;
ohodge3 0:316cfb89e9cd 54
ohodge3 0:316cfb89e9cd 55 //loop
ohodge3 0:316cfb89e9cd 56 while (1) {
ohodge3 0:316cfb89e9cd 57
ohodge3 0:316cfb89e9cd 58 if(xbee.readable()){
ohodge3 0:316cfb89e9cd 59 prev = serialChar;
ohodge3 0:316cfb89e9cd 60 serialChar = xbee.getc();
ohodge3 0:316cfb89e9cd 61
ohodge3 0:316cfb89e9cd 62
ohodge3 0:316cfb89e9cd 63 //left right rotation
ohodge3 0:316cfb89e9cd 64 if (prev == 'a' && serialChar < 60){
ohodge3 0:316cfb89e9cd 65 shoulderNum = shoulderNum + 0.01;
ohodge3 0:316cfb89e9cd 66 if (shoulderNum > 1)
ohodge3 0:316cfb89e9cd 67 shoulderNum = 1;
ohodge3 0:316cfb89e9cd 68 shoulder = shoulderNum;
ohodge3 0:316cfb89e9cd 69 led1 = 1;
ohodge3 0:316cfb89e9cd 70 }
ohodge3 0:316cfb89e9cd 71 else if (prev == 'a' && serialChar > 110){
ohodge3 0:316cfb89e9cd 72 shoulderNum = shoulderNum - 0.01;
ohodge3 0:316cfb89e9cd 73 if (shoulderNum < 0)
ohodge3 0:316cfb89e9cd 74 shoulderNum = 0;
ohodge3 0:316cfb89e9cd 75 shoulder = shoulderNum;
ohodge3 0:316cfb89e9cd 76 led2 = 1;
ohodge3 0:316cfb89e9cd 77 }
ohodge3 0:316cfb89e9cd 78 else{
ohodge3 0:316cfb89e9cd 79 shoulder = shoulderNum;
ohodge3 0:316cfb89e9cd 80 }
ohodge3 0:316cfb89e9cd 81
ohodge3 0:316cfb89e9cd 82
ohodge3 0:316cfb89e9cd 83 //up down rotation
ohodge3 0:316cfb89e9cd 84 if (prev == 'b' && serialChar < 60){
ohodge3 0:316cfb89e9cd 85 elbowNum = elbowNum + 0.01;
ohodge3 0:316cfb89e9cd 86 if (elbowNum > 1)
ohodge3 0:316cfb89e9cd 87 elbowNum = 1;
ohodge3 0:316cfb89e9cd 88 elbow = elbowNum;
ohodge3 0:316cfb89e9cd 89 led1 = 1;
ohodge3 0:316cfb89e9cd 90 }
ohodge3 0:316cfb89e9cd 91 else if (prev == 'b' && serialChar > 110){
ohodge3 0:316cfb89e9cd 92 elbowNum = elbowNum - 0.01;
ohodge3 0:316cfb89e9cd 93 if (elbowNum < 0)
ohodge3 0:316cfb89e9cd 94 elbowNum = 0;
ohodge3 0:316cfb89e9cd 95 elbow = elbowNum;
ohodge3 0:316cfb89e9cd 96 led2 = 1;
ohodge3 0:316cfb89e9cd 97 }
ohodge3 0:316cfb89e9cd 98 else{
ohodge3 0:316cfb89e9cd 99 elbow = elbowNum;
ohodge3 0:316cfb89e9cd 100 }
ohodge3 0:316cfb89e9cd 101
ohodge3 0:316cfb89e9cd 102 //motor rotation
ohodge3 0:316cfb89e9cd 103 if (prev == 'c' && serialChar < 60){
ohodge3 0:316cfb89e9cd 104 speed1 = 1.0;
ohodge3 0:316cfb89e9cd 105 speed2 = -1.0;
ohodge3 0:316cfb89e9cd 106 go();
ohodge3 0:316cfb89e9cd 107 }
ohodge3 0:316cfb89e9cd 108 else if (prev == 'c' && serialChar > 110){
ohodge3 0:316cfb89e9cd 109 speed1 = -1;
ohodge3 0:316cfb89e9cd 110 speed2 = 1;
ohodge3 0:316cfb89e9cd 111 go();
ohodge3 0:316cfb89e9cd 112 }
ohodge3 0:316cfb89e9cd 113 else if (prev == 'c' && serialChar > 60 && serialChar < 110 && speed1 != speed2){
ohodge3 0:316cfb89e9cd 114 speed1 = 0;
ohodge3 0:316cfb89e9cd 115 speed2 = 0;
ohodge3 0:316cfb89e9cd 116 go();
ohodge3 0:316cfb89e9cd 117 }
ohodge3 0:316cfb89e9cd 118
ohodge3 0:316cfb89e9cd 119 //motor forward and reverse
ohodge3 0:316cfb89e9cd 120 if (prev == 'd' && serialChar < 60){
ohodge3 0:316cfb89e9cd 121 speed1 = -1.0;
ohodge3 0:316cfb89e9cd 122 speed2 = -1.0;
ohodge3 0:316cfb89e9cd 123 go();
ohodge3 0:316cfb89e9cd 124 }
ohodge3 0:316cfb89e9cd 125 else if (prev == 'd' && serialChar > 110){
ohodge3 0:316cfb89e9cd 126 speed1 = 1;
ohodge3 0:316cfb89e9cd 127 speed2 = 1;
ohodge3 0:316cfb89e9cd 128 go();
ohodge3 0:316cfb89e9cd 129 }
ohodge3 0:316cfb89e9cd 130 else if (prev == 'd' && serialChar > 60 && serialChar < 110 && speed1 == speed2){
ohodge3 0:316cfb89e9cd 131 speed1 = 0;
ohodge3 0:316cfb89e9cd 132 speed2 = 0;
ohodge3 0:316cfb89e9cd 133 go();
ohodge3 0:316cfb89e9cd 134 }
ohodge3 0:316cfb89e9cd 135
ohodge3 0:316cfb89e9cd 136
ohodge3 0:316cfb89e9cd 137
ohodge3 0:316cfb89e9cd 138
ohodge3 0:316cfb89e9cd 139 //wrist controller
ohodge3 0:316cfb89e9cd 140 if (prev == 'e' && serialChar == 36){
ohodge3 0:316cfb89e9cd 141 wristNum = wristNum + 0.01;
ohodge3 0:316cfb89e9cd 142 if (wristNum > 1)
ohodge3 0:316cfb89e9cd 143 wristNum = 1;
ohodge3 0:316cfb89e9cd 144 wrist = wristNum;
ohodge3 0:316cfb89e9cd 145 led1 = 1;
ohodge3 0:316cfb89e9cd 146 }
ohodge3 0:316cfb89e9cd 147 else if (prev == 'f' && serialChar == 36){
ohodge3 0:316cfb89e9cd 148 wristNum = wristNum - 0.01;
ohodge3 0:316cfb89e9cd 149 if (wristNum < 0)
ohodge3 0:316cfb89e9cd 150 wristNum = 0;
ohodge3 0:316cfb89e9cd 151 wrist = wristNum;
ohodge3 0:316cfb89e9cd 152 led2 = 1;
ohodge3 0:316cfb89e9cd 153 }
ohodge3 0:316cfb89e9cd 154 else{
ohodge3 0:316cfb89e9cd 155 wrist = wristNum;
ohodge3 0:316cfb89e9cd 156 }
ohodge3 0:316cfb89e9cd 157
ohodge3 0:316cfb89e9cd 158 //print to pc for debugging (Tera Term etc.)
ohodge3 0:316cfb89e9cd 159 pc.putc(serialChar);
ohodge3 0:316cfb89e9cd 160 }
ohodge3 0:316cfb89e9cd 161 }
ohodge3 0:316cfb89e9cd 162 }