ManualControl

Dependencies:   TPixy-Interface

Fork of MbedOS_Robot_Team by ECE4333 - 2018 - Ahmed & Brandon

Files at this revision

API Documentation at this revision

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

CameraThread.cpp Show annotated file Show diff for this revision Revisions of this file
PiControlThread.cpp Show annotated file Show diff for this revision Revisions of this file
PiControlThread.h Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
ui.cpp Show annotated file Show diff for this revision Revisions of this file
--- 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;   
+    
+}