Xbox Controlled Camera Bot / Mbed 2 deprecated Camera_Bot

Dependencies:   mbed Servo mbed-rtos Motor

main.cpp@12:439c0b3f0673, 2020-04-29 (annotated)

Committer:
npatel387
Date:
Wed Apr 29 00:11:16 2020 +0000
Revision:
12:439c0b3f0673
Parent:
11:45ee4a96bca0
Child:
13:ec72079880e9
Encoder sync improvements

Who changed what in which revision?

UserRevisionLine numberNew contents of line
npatel387 2:3d1a5025c32a 1 #include "mbed.h"
npatel387 0:b52adb14e1a9 2 #include "Servo.h"
npatel387 0:b52adb14e1a9 3 #include "Motor.h"
npatel387 0:b52adb14e1a9 4 #include "rtos.h"
npatel387 0:b52adb14e1a9 5
npatel387 0:b52adb14e1a9 6 Serial pi(USBTX, USBRX);
npatel387 2:3d1a5025c32a 7 InterruptIn leftEncoder(p22);
npatel387 2:3d1a5025c32a 8 InterruptIn rightEncoder(p21);
npatel387 0:b52adb14e1a9 9 Motor leftMotor(p23, p30, p29);
npatel387 0:b52adb14e1a9 10 Motor rightMotor(p24, p5, p6);
npatel387 0:b52adb14e1a9 11 Servo topServo(p25);
npatel387 0:b52adb14e1a9 12 Servo bottomServo(p26);
npatel387 0:b52adb14e1a9 13
npatel387 1:3f064275f04d 14 volatile int xpadState = 0; //left-right Dpad
npatel387 1:3f064275f04d 15 volatile int ypadState = 0; //up-down Dpad
npatel387 2:3d1a5025c32a 16 volatile int leftJoystickState = 0;
npatel387 2:3d1a5025c32a 17 volatile int rightJoystickState = 0;
npatel387 4:22910b683dea 18 volatile int leftEncoderCount = 0; //keep track of encoder ticks
npatel387 2:3d1a5025c32a 19 volatile int rightEncoderCount = 0;
npatel387 2:3d1a5025c32a 20
npatel387 4:22910b683dea 21 //increment left encoder counts
npatel387 2:3d1a5025c32a 22 void LeftCount()
npatel387 2:3d1a5025c32a 23 {
npatel387 2:3d1a5025c32a 24 leftEncoderCount = leftEncoderCount + 1;
npatel387 2:3d1a5025c32a 25 }
npatel387 2:3d1a5025c32a 26
npatel387 4:22910b683dea 27 //increment right encoder counts
npatel387 2:3d1a5025c32a 28 void RightCount()
npatel387 2:3d1a5025c32a 29 {
npatel387 2:3d1a5025c32a 30 rightEncoderCount = rightEncoderCount + 1;
npatel387 2:3d1a5025c32a 31 }
npatel387 0:b52adb14e1a9 32
npatel387 0:b52adb14e1a9 33 //rotate the pan/tilt servos
npatel387 0:b52adb14e1a9 34 void RotateServos()
npatel387 0:b52adb14e1a9 35 {
npatel387 4:22910b683dea 36 float topServoPos = 0.0;
npatel387 4:22910b683dea 37 float bottomServoPos = 0.0;
npatel387 4:22910b683dea 38
npatel387 4:22910b683dea 39 while(1){
npatel387 7:aa95fa560952 40 if(xpadState == -1) //pan right
npatel387 0:b52adb14e1a9 41 {
npatel387 0:b52adb14e1a9 42 bottomServoPos = bottomServoPos + 0.05;
npatel387 0:b52adb14e1a9 43 if(bottomServoPos > 1.0) bottomServoPos = 1.0;
npatel387 0:b52adb14e1a9 44 }
npatel387 7:aa95fa560952 45 else if(xpadState == 1) //pan left
npatel387 0:b52adb14e1a9 46 {
npatel387 0:b52adb14e1a9 47 bottomServoPos = bottomServoPos - 0.05;
npatel387 0:b52adb14e1a9 48 if(bottomServoPos < 0.0) bottomServoPos = 0.0;
npatel387 0:b52adb14e1a9 49 }
npatel387 0:b52adb14e1a9 50
npatel387 4:22910b683dea 51 if(ypadState == 1) //tilt up
npatel387 0:b52adb14e1a9 52 {
npatel387 0:b52adb14e1a9 53 topServoPos = topServoPos + 0.05;
npatel387 0:b52adb14e1a9 54 if(topServoPos > 1.0) topServoPos = 1.0;
npatel387 0:b52adb14e1a9 55 }
npatel387 4:22910b683dea 56 else if(ypadState == -1) //tilt down
npatel387 0:b52adb14e1a9 57 {
npatel387 0:b52adb14e1a9 58 topServoPos = topServoPos - 0.05;
npatel387 0:b52adb14e1a9 59 if(topServoPos < 0.0) topServoPos = 0.0;
npatel387 0:b52adb14e1a9 60 }
npatel387 0:b52adb14e1a9 61
npatel387 0:b52adb14e1a9 62 topServo = topServoPos;
npatel387 0:b52adb14e1a9 63 bottomServo = bottomServoPos;
npatel387 0:b52adb14e1a9 64 Thread::wait(150);
npatel387 4:22910b683dea 65 }
npatel387 0:b52adb14e1a9 66 }
npatel387 0:b52adb14e1a9 67
npatel387 0:b52adb14e1a9 68
npatel387 0:b52adb14e1a9 69 //Sets and keeps motor speed matched using encoder output
npatel387 0:b52adb14e1a9 70 void SpeedBalance()
npatel387 0:b52adb14e1a9 71 {
npatel387 4:22910b683dea 72 bool interruptAttached = false; //flag indicating whether encoders are being counted
npatel387 6:0597a4ac4696 73 bool speedSet = false; //flag indicating if the initial motor speed has been set before switching to encoder control
npatel387 4:22910b683dea 74
npatel387 4:22910b683dea 75 //used when speed adjusting using encoders
npatel387 4:22910b683dea 76 float leftMotorSpeed = 0.0;
npatel387 4:22910b683dea 77 float rightMotorSpeed = 0.0;
npatel387 4:22910b683dea 78
npatel387 4:22910b683dea 79 int encoderCountDifference = 0;
npatel387 12:439c0b3f0673 80 const int countDifferenceTolerance = 8; //How many counts difference between encoders is allowed
npatel387 4:22910b683dea 81
npatel387 4:22910b683dea 82 while(1){
npatel387 2:3d1a5025c32a 83 if(leftJoystickState != 0 && rightJoystickState != 0 && leftJoystickState == rightJoystickState) //only need to match speeds if they're moving in same direction
npatel387 0:b52adb14e1a9 84 {
npatel387 4:22910b683dea 85 if(speedSet == false) //set motor speed initally
npatel387 4:22910b683dea 86 {
npatel387 4:22910b683dea 87 leftMotorSpeed = leftJoystickState;
npatel387 10:1402608022be 88 rightMotorSpeed = -1*rightJoystickState;
npatel387 4:22910b683dea 89 speedSet = true;
npatel387 4:22910b683dea 90 }
npatel387 4:22910b683dea 91
npatel387 4:22910b683dea 92 //start counting encoders if not already
npatel387 2:3d1a5025c32a 93 if(interruptAttached == false)
npatel387 2:3d1a5025c32a 94 {
npatel387 2:3d1a5025c32a 95 leftEncoder.fall(&LeftCount);
npatel387 2:3d1a5025c32a 96 rightEncoder.fall(&RightCount);
npatel387 2:3d1a5025c32a 97 interruptAttached = true;
npatel387 2:3d1a5025c32a 98 }
npatel387 0:b52adb14e1a9 99
npatel387 12:439c0b3f0673 100 Thread::wait(1000);
npatel387 4:22910b683dea 101 encoderCountDifference = leftEncoderCount - rightEncoderCount;
npatel387 4:22910b683dea 102 if(encoderCountDifference > countDifferenceTolerance) //if left encoder counted more, left motor is faster
npatel387 2:3d1a5025c32a 103 {
npatel387 6:0597a4ac4696 104 //right motor stays constant, left motor slows down to follow it
npatel387 10:1402608022be 105 rightMotor.speed(-1*rightJoystickState);
npatel387 6:0597a4ac4696 106
npatel387 4:22910b683dea 107 //add or subtract motor speed, depending on if its positive or negative
npatel387 4:22910b683dea 108 if(leftMotorSpeed < 0){
npatel387 12:439c0b3f0673 109 leftMotorSpeed = leftMotorSpeed + 0.001;
npatel387 4:22910b683dea 110 leftMotor.speed(leftMotorSpeed);
npatel387 4:22910b683dea 111 }
npatel387 4:22910b683dea 112 else if(leftMotorSpeed > 0){
npatel387 12:439c0b3f0673 113 leftMotorSpeed = leftMotorSpeed - 0.001;
npatel387 4:22910b683dea 114 leftMotor.speed(leftMotorSpeed);
npatel387 4:22910b683dea 115 }
npatel387 4:22910b683dea 116 leftEncoderCount = 0; //reset encoder counts
npatel387 2:3d1a5025c32a 117 rightEncoderCount = 0;
npatel387 9:431b67beb44d 118 Thread::yield();
npatel387 2:3d1a5025c32a 119 }
npatel387 4:22910b683dea 120 else if(encoderCountDifference < (-1 * countDifferenceTolerance)) //if left encoder counted less, right motor is faster
npatel387 2:3d1a5025c32a 121 {
npatel387 6:0597a4ac4696 122 //left motor speed stays constant, right motor slows down to follow it
npatel387 6:0597a4ac4696 123 leftMotor.speed(leftJoystickState);
npatel387 6:0597a4ac4696 124
npatel387 4:22910b683dea 125 //add or subtract motor speed, depending on if its positive or negative
npatel387 4:22910b683dea 126 if(leftMotorSpeed < 0){
npatel387 12:439c0b3f0673 127 rightMotorSpeed = rightMotorSpeed + 0.001;
npatel387 11:45ee4a96bca0 128 rightMotor.speed(rightMotorSpeed);
npatel387 4:22910b683dea 129 }
npatel387 4:22910b683dea 130 else if(leftMotorSpeed > 0){
npatel387 12:439c0b3f0673 131 rightMotorSpeed = rightMotorSpeed - 0.001;
npatel387 11:45ee4a96bca0 132 rightMotor.speed(rightMotorSpeed);
npatel387 4:22910b683dea 133 }
npatel387 4:22910b683dea 134 leftEncoderCount = 0; //reset encoder counts
npatel387 2:3d1a5025c32a 135 rightEncoderCount = 0;
npatel387 8:c59f12a4960f 136 Thread::yield();
npatel387 2:3d1a5025c32a 137 }
npatel387 8:c59f12a4960f 138 else //if counts within tolerances, keep speeds the same
npatel387 8:c59f12a4960f 139 {
npatel387 8:c59f12a4960f 140 rightMotor.speed(rightMotorSpeed);
npatel387 8:c59f12a4960f 141 leftMotor.speed(leftMotorSpeed);
npatel387 8:c59f12a4960f 142 Thread::yield(); //if counts are same, nothing left to do
npatel387 8:c59f12a4960f 143 }
npatel387 0:b52adb14e1a9 144 }
npatel387 4:22910b683dea 145 else //The case where joysticks arent in the same direction
npatel387 2:3d1a5025c32a 146 {
npatel387 4:22910b683dea 147 //set motors using joystick states, since motor speed wont need to be dynamically changed in this case
npatel387 2:3d1a5025c32a 148 leftMotor.speed(leftJoystickState);
npatel387 10:1402608022be 149 rightMotor.speed(-1*rightJoystickState);
npatel387 4:22910b683dea 150 speedSet = false;
npatel387 4:22910b683dea 151
npatel387 4:22910b683dea 152 //Stop counting encoders because they aren't being used
npatel387 2:3d1a5025c32a 153 if(interruptAttached == true)
npatel387 2:3d1a5025c32a 154 {
npatel387 2:3d1a5025c32a 155 leftEncoder.fall(NULL);
npatel387 2:3d1a5025c32a 156 rightEncoder.fall(NULL);
npatel387 2:3d1a5025c32a 157 interruptAttached = false;
npatel387 2:3d1a5025c32a 158 leftEncoderCount = 0;
npatel387 2:3d1a5025c32a 159 rightEncoderCount = 0;
npatel387 2:3d1a5025c32a 160 }
npatel387 8:c59f12a4960f 161 Thread::yield();
npatel387 2:3d1a5025c32a 162 }
npatel387 4:22910b683dea 163 }
npatel387 0:b52adb14e1a9 164 }
npatel387 0:b52adb14e1a9 165
npatel387 0:b52adb14e1a9 166 int main()
npatel387 0:b52adb14e1a9 167 {
npatel387 0:b52adb14e1a9 168 Thread motorControl(SpeedBalance);
npatel387 0:b52adb14e1a9 169 Thread servoControl(RotateServos);
npatel387 0:b52adb14e1a9 170
npatel387 0:b52adb14e1a9 171 char servoNum = '0';
npatel387 0:b52adb14e1a9 172 char leftMotNum = '0';
npatel387 0:b52adb14e1a9 173 char rightMotNum = '0';
npatel387 6:0597a4ac4696 174 char initialCheck = '0';
npatel387 0:b52adb14e1a9 175
npatel387 0:b52adb14e1a9 176 while(1)
npatel387 0:b52adb14e1a9 177 {
npatel387 0:b52adb14e1a9 178 if(pi.readable()) //check if new command from pi available
npatel387 0:b52adb14e1a9 179 {
npatel387 6:0597a4ac4696 180 initialCheck = pi.getc();
npatel387 6:0597a4ac4696 181 if (initialCheck == '!' ) { //look for dpad update
npatel387 0:b52adb14e1a9 182 servoNum = pi.getc();
npatel387 4:22910b683dea 183 if (servoNum == '0') { //dpad released
npatel387 1:3f064275f04d 184 xpadState = 0;
npatel387 1:3f064275f04d 185 ypadState = 0;
npatel387 1:3f064275f04d 186 }
npatel387 4:22910b683dea 187 else if (servoNum == '1') { //dpad left
npatel387 0:b52adb14e1a9 188 xpadState = -1;
npatel387 0:b52adb14e1a9 189 }
npatel387 4:22910b683dea 190 else if (servoNum == '2') { //dpad right
npatel387 0:b52adb14e1a9 191 xpadState = 1;
npatel387 0:b52adb14e1a9 192 }
npatel387 4:22910b683dea 193 else if (servoNum == '3') { //dpad up
npatel387 0:b52adb14e1a9 194 ypadState = 1;
npatel387 0:b52adb14e1a9 195 }
npatel387 4:22910b683dea 196 else if (servoNum == '4') { //dpad down
npatel387 0:b52adb14e1a9 197 ypadState = -1;
npatel387 0:b52adb14e1a9 198 }
npatel387 0:b52adb14e1a9 199 }
npatel387 6:0597a4ac4696 200 else if (initialCheck == '(') { //look for left joystick update
npatel387 0:b52adb14e1a9 201 leftMotNum = pi.getc();
npatel387 4:22910b683dea 202 if (leftMotNum == '0') { //joystick center
npatel387 0:b52adb14e1a9 203 leftJoystickState = 0;
npatel387 0:b52adb14e1a9 204 }
npatel387 4:22910b683dea 205 else if (leftMotNum == '1') { //joystick up
npatel387 0:b52adb14e1a9 206 leftJoystickState = 1;
npatel387 0:b52adb14e1a9 207 }
npatel387 4:22910b683dea 208 else if (leftMotNum == '2') { //joystick down
npatel387 0:b52adb14e1a9 209 leftJoystickState = -1;
npatel387 0:b52adb14e1a9 210 }
npatel387 0:b52adb14e1a9 211 }
npatel387 6:0597a4ac4696 212 else if (initialCheck == ')' ) { //look for right joystick update
npatel387 0:b52adb14e1a9 213 rightMotNum = pi.getc();
npatel387 4:22910b683dea 214 if (rightMotNum == '0') { //joystick center
npatel387 2:3d1a5025c32a 215 rightJoystickState = 0;
npatel387 0:b52adb14e1a9 216 }
npatel387 4:22910b683dea 217 else if (rightMotNum == '1') { //joystick up
npatel387 2:3d1a5025c32a 218 rightJoystickState = 1;
npatel387 0:b52adb14e1a9 219 }
npatel387 4:22910b683dea 220 else if (rightMotNum == '2') { //joystick down
npatel387 2:3d1a5025c32a 221 rightJoystickState = -1;
npatel387 0:b52adb14e1a9 222 }
npatel387 0:b52adb14e1a9 223 }
npatel387 0:b52adb14e1a9 224 }
npatel387 0:b52adb14e1a9 225 }
npatel387 0:b52adb14e1a9 226 }