ManualControl
Dependencies: TPixy-Interface
Fork of MbedOS_Robot by
Revision 17:1184df616383, committed 2018-03-11
- Comitter:
- asobhy
- Date:
- Sun Mar 11 01:26:23 2018 +0000
- Parent:
- 16:58ec2b891a25
- Child:
- 18:db6d9fc1ebd0
- Commit message:
- MANUAL CONTROL - TESTING AND TUNNING REQUIRED
Changed in this revision
--- a/CameraThread.cpp Sun Mar 11 00:37:58 2018 +0000 +++ b/CameraThread.cpp Sun Mar 11 01:26:23 2018 +0000 @@ -84,7 +84,7 @@ while (true) { - + osSignalWait(0x01, osWaitForever); // Go to sleep until signal is received. blocksR = pixyR.getBlocks();
--- a/PiControlThread.cpp Sun Mar 11 00:37:58 2018 +0000 +++ b/PiControlThread.cpp Sun Mar 11 01:26:23 2018 +0000 @@ -41,12 +41,12 @@ // steering gain float Ks = 0.15; - // distance gain float Kd = 10; // overall robot required speed int Setpoint; +Mutex mutexSetpoint; osThreadId PiControlId; @@ -125,7 +125,7 @@ // If distance increase then speed should increase. // If object is moving away from the the robot increase robot speed - cameraData_mutex.lock(); + /*cameraData_mutex.lock(); if(DistanceError > 0) { // Proportional controller @@ -142,7 +142,14 @@ setpointR = Setpoint + (Ks*SteeringError); setpointL = Setpoint - (Ks*SteeringError); cameraData_mutex.unlock(); + */ + /********************Manual Setpoint and Steering**********************/ + mutexSetpoint.lock(); + setpointR = Setpoint + (Ks*SteeringError); + setpointL = Setpoint - (Ks*SteeringError); + mutexSetpoint.unlock(); + // Make sure our limit is not exceeded setpointR = SaturateValue(setpointR, 560); setpointL = SaturateValue(setpointL, 560); @@ -162,7 +169,7 @@ motorDriver_R_reverse(U_right); } - // Set speed and direction of left motor + // Set speed and direction for left motor if (U_left >= 0) { motorDriver_L_forward(U_left);
--- a/PiControlThread.h Sun Mar 11 00:37:58 2018 +0000 +++ b/PiControlThread.h Sun Mar 11 01:26:23 2018 +0000 @@ -9,6 +9,10 @@ #ifndef PERIODIC_INT_H #define PERIODIC_INT_H +// overall robot required speed +extern int Setpoint; +extern Mutex mutexSetpoint; + extern osThreadId PiControlId; void PiControlThreadInit(void);
--- a/main.cpp Sun Mar 11 00:37:58 2018 +0000 +++ b/main.cpp Sun Mar 11 01:26:23 2018 +0000 @@ -30,7 +30,7 @@ // Initialize and run the threads below: WatchdogThreadInit(); - CameraThreadInit(); + //CameraThreadInit(); PiControlThreadInit(); ExternalInterruptThreadInit();
--- a/ui.cpp Sun Mar 11 00:37:58 2018 +0000 +++ b/ui.cpp Sun Mar 11 01:26:23 2018 +0000 @@ -14,6 +14,7 @@ #include "WatchdogThread.h" #include "ui.h" #include "CameraThread.h" +#include "PiControlThread.h" Serial bluetooth(p9,p10); // Define the bluetooth channel and IO pins Serial pc(USBTX, USBRX); // Pins (tx, rx) for PC serial channel @@ -25,12 +26,13 @@ int setpointR = 0; int setpointL = 0; + bool killRobot = false; // variable to store character recieved from terminal char x; +int16_t position; -int16_t position; /****************************************************************************** User interface 3 @@ -45,14 +47,16 @@ // safety mechanism if (bluetooth.readable()){ x = bluetooth.getc(); - if (x == 'r') + if (x == 't') { killRobot = true; } - else if (x == 's') + else if (x == 'r') { killRobot = false; } + + } } @@ -259,4 +263,65 @@ } +/****************************************************************************** + User interface 3 - manual control +******************************************************************************/ +void consoleUIManualControl(void) +{ + + if (bluetooth.readable()) { + x = bluetooth.getc(); + + // if input from console is the letter 'r' + if(x == 'r') { + // reset watchdog timer + WatchdogReset(); + setpointR = 0; + setpointL = 0; + bluetooth.printf("\r\nWatchdog has been reset"); + } + +/******************************ROBOT FWD RVS***********************************/ + // if w is pressed increase the speed + // by incrementing u + else if(x == 'w') { + mutexSetpoint.lock(); + Setpoint = Setpoint + 100; + mutexSetpoint.unlock(); + + } + + // if s is pressed decrease the speed + // by decrementing u + else if(x == 's') { + mutexSetpoint.lock(); + Setpoint = Setpoint - 100; + mutexSetpoint.unlock(); + + } + +/******************************ROBOT STEERING**********************************/ + else if (x=='d') + { + mutexSetpoint.lock(); + SteeringError = SteeringError + 10; + mutexSetpoint.unlock(); + } + else if (x=='a') + { + mutexSetpoint.lock(); + SteeringError = SteeringError - 10; + mutexSetpoint.unlock(); + } + // error wrong input + else { + bluetooth.printf("\r\nwrong input please enter \'w\' to increase the speed, \'s\' to reduce it or move in the opposite direction and \'r\' to reset the watchdog"); + } + } + + // If no key is pressed stop the robot. + Setpoint = 0; + SteeringError = 0; + +}