Xbox Controlled Camera Bot / Mbed 2 deprecated Camera_Bot

Dependencies:   mbed Servo mbed-rtos Motor

main.cpp@3:80579b8865db, 2020-04-27 (annotated)

Committer:
npatel387
Date:
Mon Apr 27 01:30:30 2020 +0000
Revision:
3:80579b8865db
Parent:
1:3f064275f04d
Child:
5:8500e9326652
Added loops to thread functions

Who changed what in which revision?

UserRevisionLine numberNew contents of line
npatel387 0:b52adb14e1a9 1 #include "mbed.h"#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 0:b52adb14e1a9 7 DigitalIn leftEncoder(p22);
npatel387 0:b52adb14e1a9 8 DigitalIn 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 0:b52adb14e1a9 14
npatel387 0:b52adb14e1a9 15 volatile float topServoPos = 0.0;
npatel387 0:b52adb14e1a9 16 volatile float bottomServoPos = 0.0;
npatel387 1:3f064275f04d 17 volatile int xpadState = 0; //left-right Dpad
npatel387 1:3f064275f04d 18 volatile int ypadState = 0; //up-down Dpad
npatel387 0:b52adb14e1a9 19 volatile float leftJoystickState = 0.0;
npatel387 0:b52adb14e1a9 20 volatile float rightJoystickState = 0.0;
npatel387 0:b52adb14e1a9 21
npatel387 0:b52adb14e1a9 22 //rotate the pan/tilt servos
npatel387 0:b52adb14e1a9 23 void RotateServos()
npatel387 0:b52adb14e1a9 24 {
npatel387 3:80579b8865db 25 while(1){
npatel387 0:b52adb14e1a9 26 if(xpadState == 1)
npatel387 0:b52adb14e1a9 27 {
npatel387 0:b52adb14e1a9 28 bottomServoPos = bottomServoPos + 0.05;
npatel387 0:b52adb14e1a9 29 if(bottomServoPos > 1.0) bottomServoPos = 1.0;
npatel387 0:b52adb14e1a9 30 }
npatel387 0:b52adb14e1a9 31 else if(xpadState == -1)
npatel387 0:b52adb14e1a9 32 {
npatel387 0:b52adb14e1a9 33 bottomServoPos = bottomServoPos - 0.05;
npatel387 0:b52adb14e1a9 34 if(bottomServoPos < 0.0) bottomServoPos = 0.0;
npatel387 0:b52adb14e1a9 35 }
npatel387 0:b52adb14e1a9 36
npatel387 0:b52adb14e1a9 37 if(ypadState == 1)
npatel387 0:b52adb14e1a9 38 {
npatel387 0:b52adb14e1a9 39 topServoPos = topServoPos + 0.05;
npatel387 0:b52adb14e1a9 40 if(topServoPos > 1.0) topServoPos = 1.0;
npatel387 0:b52adb14e1a9 41 }
npatel387 0:b52adb14e1a9 42 else if(xpadState == -1)
npatel387 0:b52adb14e1a9 43 {
npatel387 0:b52adb14e1a9 44 topServoPos = topServoPos - 0.05;
npatel387 0:b52adb14e1a9 45 if(topServoPos < 0.0) topServoPos = 0.0;
npatel387 0:b52adb14e1a9 46 }
npatel387 0:b52adb14e1a9 47
npatel387 0:b52adb14e1a9 48 topServo = topServoPos;
npatel387 0:b52adb14e1a9 49 bottomServo = bottomServoPos;
npatel387 0:b52adb14e1a9 50 Thread::wait(150);
npatel387 3:80579b8865db 51 }
npatel387 0:b52adb14e1a9 52 }
npatel387 0:b52adb14e1a9 53
npatel387 0:b52adb14e1a9 54
npatel387 0:b52adb14e1a9 55 //Sets and keeps motor speed matched using encoder output
npatel387 0:b52adb14e1a9 56 void SpeedBalance()
npatel387 0:b52adb14e1a9 57 {
npatel387 3:80579b8865db 58 while(1){
npatel387 0:b52adb14e1a9 59 leftMotor.speed(leftJoystickState);
npatel387 0:b52adb14e1a9 60 rightMotor.speed(rightJoystickState);
npatel387 1:3f064275f04d 61 if(leftJoystickState != 0.0 && rightJoystickState != 0.0 && leftJoystickState == rightJoystickState) //only need to match speeds if they're moving in same direction
npatel387 0:b52adb14e1a9 62 {
npatel387 0:b52adb14e1a9 63
npatel387 0:b52adb14e1a9 64 }
npatel387 3:80579b8865db 65 }
npatel387 0:b52adb14e1a9 66 }
npatel387 0:b52adb14e1a9 67
npatel387 0:b52adb14e1a9 68 int main()
npatel387 0:b52adb14e1a9 69 {
npatel387 0:b52adb14e1a9 70 Thread motorControl(SpeedBalance);
npatel387 0:b52adb14e1a9 71 Thread servoControl(RotateServos);
npatel387 0:b52adb14e1a9 72
npatel387 0:b52adb14e1a9 73 char servoNum = '0';
npatel387 0:b52adb14e1a9 74 char leftMotNum = '0';
npatel387 0:b52adb14e1a9 75 char rightMotNum = '0';
npatel387 0:b52adb14e1a9 76
npatel387 0:b52adb14e1a9 77 while(1)
npatel387 0:b52adb14e1a9 78 {
npatel387 0:b52adb14e1a9 79 if(pi.readable()) //check if new command from pi available
npatel387 0:b52adb14e1a9 80 {
npatel387 0:b52adb14e1a9 81 if (pi.getc() == '!' ) {
npatel387 0:b52adb14e1a9 82 servoNum = pi.getc();
npatel387 1:3f064275f04d 83 if (servoNum == '0') {
npatel387 1:3f064275f04d 84 xpadState = 0;
npatel387 1:3f064275f04d 85 ypadState = 0;
npatel387 1:3f064275f04d 86 }
npatel387 1:3f064275f04d 87 else if (servoNum == '1') {
npatel387 0:b52adb14e1a9 88 xpadState = -1;
npatel387 0:b52adb14e1a9 89 }
npatel387 1:3f064275f04d 90 else if (servoNum == '2') {
npatel387 0:b52adb14e1a9 91 xpadState = 1;
npatel387 0:b52adb14e1a9 92 }
npatel387 1:3f064275f04d 93 else if (servoNum == '3') {
npatel387 0:b52adb14e1a9 94 ypadState = 1;
npatel387 0:b52adb14e1a9 95 }
npatel387 1:3f064275f04d 96 else if (servoNum == '4') {
npatel387 0:b52adb14e1a9 97 ypadState = -1;
npatel387 0:b52adb14e1a9 98 }
npatel387 0:b52adb14e1a9 99 }
npatel387 0:b52adb14e1a9 100 else if (pi.getc() == '(') {
npatel387 0:b52adb14e1a9 101 leftMotNum = pi.getc();
npatel387 0:b52adb14e1a9 102 if (leftMotNum == '0') {
npatel387 0:b52adb14e1a9 103 leftJoystickState = 0;
npatel387 0:b52adb14e1a9 104 }
npatel387 1:3f064275f04d 105 else if (leftMotNum == '1') {
npatel387 0:b52adb14e1a9 106 leftJoystickState = 1;
npatel387 0:b52adb14e1a9 107 }
npatel387 1:3f064275f04d 108 else if (leftMotNum == '2') {
npatel387 0:b52adb14e1a9 109 leftJoystickState = -1;
npatel387 0:b52adb14e1a9 110 }
npatel387 0:b52adb14e1a9 111 }
npatel387 0:b52adb14e1a9 112 else if (pi.getc() == ')' ) {
npatel387 0:b52adb14e1a9 113 rightMotNum = pi.getc();
npatel387 0:b52adb14e1a9 114 if (rightMotNum == '0') {
npatel387 1:3f064275f04d 115 rightJoystickState = 0.0;
npatel387 0:b52adb14e1a9 116 }
npatel387 1:3f064275f04d 117 else if (rightMotNum == '1') {
npatel387 1:3f064275f04d 118 rightJoystickState = 1.0;
npatel387 0:b52adb14e1a9 119 }
npatel387 1:3f064275f04d 120 else if (rightMotNum == '2') {
npatel387 1:3f064275f04d 121 rightJoystickState = -1.0;
npatel387 0:b52adb14e1a9 122 }
npatel387 0:b52adb14e1a9 123 }
npatel387 0:b52adb14e1a9 124 }
npatel387 1:3f064275f04d 125 Thread::yield();
npatel387 0:b52adb14e1a9 126 }
npatel387 0:b52adb14e1a9 127 }