Xbox Controlled Camera Bot / Mbed 2 deprecated Camera_Bot

Dependencies:   mbed Servo mbed-rtos Motor

main.cpp@7:aa95fa560952, 2020-04-27 (annotated)

Committer:
npatel387
Date:
Mon Apr 27 20:43:58 2020 +0000
Revision:
7:aa95fa560952
Parent:
6:0597a4ac4696
Child:
8:c59f12a4960f
swapped bottom servo left/right orientation;

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