Xbox Controlled Camera Bot / Mbed 2 deprecated Camera_Bot

Dependencies:   mbed Servo mbed-rtos Motor

main.cpp@8:c59f12a4960f, 2020-04-27 (annotated)

Committer:
npatel387
Date:
Mon Apr 27 20:53:59 2020 +0000
Revision:
8:c59f12a4960f
Parent:
7:aa95fa560952
Child:
9:431b67beb44d
Added case in speed control where motor rotation count is the same

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 4:22910b683dea 80 int countDifferenceTolerance = 2; //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 4:22910b683dea 88 rightMotorSpeed = 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 4:22910b683dea 100 encoderCountDifference = leftEncoderCount - rightEncoderCount;
npatel387 4:22910b683dea 101 if(encoderCountDifference > countDifferenceTolerance) //if left encoder counted more, left motor is faster
npatel387 2:3d1a5025c32a 102 {
npatel387 6:0597a4ac4696 103 //right motor stays constant, left motor slows down to follow it
npatel387 6:0597a4ac4696 104 rightMotor.speed(rightJoystickState);
npatel387 6:0597a4ac4696 105
npatel387 4:22910b683dea 106 //add or subtract motor speed, depending on if its positive or negative
npatel387 4:22910b683dea 107 if(leftMotorSpeed < 0){
npatel387 4:22910b683dea 108 leftMotorSpeed = leftMotorSpeed + 0.01;
npatel387 4:22910b683dea 109 leftMotor.speed(leftMotorSpeed);
npatel387 4:22910b683dea 110 }
npatel387 4:22910b683dea 111 else if(leftMotorSpeed > 0){
npatel387 4:22910b683dea 112 leftMotorSpeed = leftMotorSpeed - 0.01;
npatel387 4:22910b683dea 113 leftMotor.speed(leftMotorSpeed);
npatel387 4:22910b683dea 114 }
npatel387 4:22910b683dea 115 leftEncoderCount = 0; //reset encoder counts
npatel387 2:3d1a5025c32a 116 rightEncoderCount = 0;
npatel387 8:c59f12a4960f 117 Thread:yield();
npatel387 2:3d1a5025c32a 118 }
npatel387 4:22910b683dea 119 else if(encoderCountDifference < (-1 * countDifferenceTolerance)) //if left encoder counted less, right motor is faster
npatel387 2:3d1a5025c32a 120 {
npatel387 6:0597a4ac4696 121 //left motor speed stays constant, right motor slows down to follow it
npatel387 6:0597a4ac4696 122 leftMotor.speed(leftJoystickState);
npatel387 6:0597a4ac4696 123
npatel387 4:22910b683dea 124 //add or subtract motor speed, depending on if its positive or negative
npatel387 4:22910b683dea 125 if(leftMotorSpeed < 0){
npatel387 4:22910b683dea 126 rightMotorSpeed = rightMotorSpeed + 0.01;
npatel387 4:22910b683dea 127 rightMotor.speed(leftMotorSpeed);
npatel387 4:22910b683dea 128 }
npatel387 4:22910b683dea 129 else if(leftMotorSpeed > 0){
npatel387 4:22910b683dea 130 rightMotorSpeed = rightMotorSpeed - 0.01;
npatel387 4:22910b683dea 131 rightMotor.speed(leftMotorSpeed);
npatel387 4:22910b683dea 132 }
npatel387 4:22910b683dea 133 leftEncoderCount = 0; //reset encoder counts
npatel387 2:3d1a5025c32a 134 rightEncoderCount = 0;
npatel387 8:c59f12a4960f 135 Thread::yield();
npatel387 2:3d1a5025c32a 136 }
npatel387 8:c59f12a4960f 137 else //if counts within tolerances, keep speeds the same
npatel387 8:c59f12a4960f 138 {
npatel387 8:c59f12a4960f 139 rightMotor.speed(rightMotorSpeed);
npatel387 8:c59f12a4960f 140 leftMotor.speed(leftMotorSpeed);
npatel387 8:c59f12a4960f 141 Thread::yield(); //if counts are same, nothing left to do
npatel387 8:c59f12a4960f 142 }
npatel387 0:b52adb14e1a9 143 }
npatel387 4:22910b683dea 144 else //The case where joysticks arent in the same direction
npatel387 2:3d1a5025c32a 145 {
npatel387 4:22910b683dea 146 //set motors using joystick states, since motor speed wont need to be dynamically changed in this case
npatel387 2:3d1a5025c32a 147 leftMotor.speed(leftJoystickState);
npatel387 2:3d1a5025c32a 148 rightMotor.speed(rightJoystickState);
npatel387 4:22910b683dea 149 speedSet = false;
npatel387 4:22910b683dea 150
npatel387 4:22910b683dea 151 //Stop counting encoders because they aren't being used
npatel387 2:3d1a5025c32a 152 if(interruptAttached == true)
npatel387 2:3d1a5025c32a 153 {
npatel387 2:3d1a5025c32a 154 leftEncoder.fall(NULL);
npatel387 2:3d1a5025c32a 155 rightEncoder.fall(NULL);
npatel387 2:3d1a5025c32a 156 interruptAttached = false;
npatel387 2:3d1a5025c32a 157 leftEncoderCount = 0;
npatel387 2:3d1a5025c32a 158 rightEncoderCount = 0;
npatel387 2:3d1a5025c32a 159 }
npatel387 8:c59f12a4960f 160 Thread::yield();
npatel387 2:3d1a5025c32a 161 }
npatel387 4:22910b683dea 162 }
npatel387 0:b52adb14e1a9 163 }
npatel387 0:b52adb14e1a9 164
npatel387 0:b52adb14e1a9 165 int main()
npatel387 0:b52adb14e1a9 166 {
npatel387 0:b52adb14e1a9 167 Thread motorControl(SpeedBalance);
npatel387 0:b52adb14e1a9 168 Thread servoControl(RotateServos);
npatel387 0:b52adb14e1a9 169
npatel387 0:b52adb14e1a9 170 char servoNum = '0';
npatel387 0:b52adb14e1a9 171 char leftMotNum = '0';
npatel387 0:b52adb14e1a9 172 char rightMotNum = '0';
npatel387 6:0597a4ac4696 173 char initialCheck = '0';
npatel387 0:b52adb14e1a9 174
npatel387 0:b52adb14e1a9 175 while(1)
npatel387 0:b52adb14e1a9 176 {
npatel387 0:b52adb14e1a9 177 if(pi.readable()) //check if new command from pi available
npatel387 0:b52adb14e1a9 178 {
npatel387 6:0597a4ac4696 179 initialCheck = pi.getc();
npatel387 6:0597a4ac4696 180 if (initialCheck == '!' ) { //look for dpad update
npatel387 0:b52adb14e1a9 181 servoNum = pi.getc();
npatel387 4:22910b683dea 182 if (servoNum == '0') { //dpad released
npatel387 1:3f064275f04d 183 xpadState = 0;
npatel387 1:3f064275f04d 184 ypadState = 0;
npatel387 1:3f064275f04d 185 }
npatel387 4:22910b683dea 186 else if (servoNum == '1') { //dpad left
npatel387 0:b52adb14e1a9 187 xpadState = -1;
npatel387 0:b52adb14e1a9 188 }
npatel387 4:22910b683dea 189 else if (servoNum == '2') { //dpad right
npatel387 0:b52adb14e1a9 190 xpadState = 1;
npatel387 0:b52adb14e1a9 191 }
npatel387 4:22910b683dea 192 else if (servoNum == '3') { //dpad up
npatel387 0:b52adb14e1a9 193 ypadState = 1;
npatel387 0:b52adb14e1a9 194 }
npatel387 4:22910b683dea 195 else if (servoNum == '4') { //dpad down
npatel387 0:b52adb14e1a9 196 ypadState = -1;
npatel387 0:b52adb14e1a9 197 }
npatel387 0:b52adb14e1a9 198 }
npatel387 6:0597a4ac4696 199 else if (initialCheck == '(') { //look for left joystick update
npatel387 0:b52adb14e1a9 200 leftMotNum = pi.getc();
npatel387 4:22910b683dea 201 if (leftMotNum == '0') { //joystick center
npatel387 0:b52adb14e1a9 202 leftJoystickState = 0;
npatel387 0:b52adb14e1a9 203 }
npatel387 4:22910b683dea 204 else if (leftMotNum == '1') { //joystick up
npatel387 0:b52adb14e1a9 205 leftJoystickState = 1;
npatel387 0:b52adb14e1a9 206 }
npatel387 4:22910b683dea 207 else if (leftMotNum == '2') { //joystick down
npatel387 0:b52adb14e1a9 208 leftJoystickState = -1;
npatel387 0:b52adb14e1a9 209 }
npatel387 0:b52adb14e1a9 210 }
npatel387 6:0597a4ac4696 211 else if (initialCheck == ')' ) { //look for right joystick update
npatel387 0:b52adb14e1a9 212 rightMotNum = pi.getc();
npatel387 4:22910b683dea 213 if (rightMotNum == '0') { //joystick center
npatel387 2:3d1a5025c32a 214 rightJoystickState = 0;
npatel387 0:b52adb14e1a9 215 }
npatel387 4:22910b683dea 216 else if (rightMotNum == '1') { //joystick up
npatel387 2:3d1a5025c32a 217 rightJoystickState = 1;
npatel387 0:b52adb14e1a9 218 }
npatel387 4:22910b683dea 219 else if (rightMotNum == '2') { //joystick down
npatel387 2:3d1a5025c32a 220 rightJoystickState = -1;
npatel387 0:b52adb14e1a9 221 }
npatel387 0:b52adb14e1a9 222 }
npatel387 0:b52adb14e1a9 223 }
npatel387 0:b52adb14e1a9 224 }
npatel387 0:b52adb14e1a9 225 }