Xbox Controlled Camera Bot / Mbed 2 deprecated Camera_Bot

Dependencies:   mbed Servo mbed-rtos Motor

main.cpp@4:22910b683dea, 2020-04-27 (annotated)

Committer:
npatel387
Date:
Mon Apr 27 02:44:10 2020 +0000
Revision:
4:22910b683dea
Parent:
2:3d1a5025c32a
Child:
6:0597a4ac4696
Encoder motor speed equalization added (untested)

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