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:
Sat Mar 03 04:47:26 2018 +0000
Parent:
14:5777377537a2
Child:
16:58ec2b891a25
Commit message:
following robot almost done. Need to get a better object to follow. Need to use a torch to light up the object. Need to adjust width of object in camera settings to match new object then test.

Changed in this revision

CameraThread.cpp Show annotated file Show diff for this revision Revisions of this file
CameraThread.h Show annotated file Show diff for this revision Revisions of this file
Drivers/PiController.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
ui.h Show annotated file Show diff for this revision Revisions of this file
--- a/CameraThread.cpp	Sat Mar 03 02:14:40 2018 +0000
+++ b/CameraThread.cpp	Sat Mar 03 04:47:26 2018 +0000
@@ -15,15 +15,13 @@
 
 #include "mbed.h"
 #include "Pixy.h"
+#include "ui.h"
 
 #define CENTER              160
 #define DISTANCE            20
 #define SIGNATURE_IN_USE    0
 
 osThreadId CameraId;
-
-extern Serial bluetooth;
-
 Mutex cameraData_mutex;
 
 SPI spiR(p11, p12, p13); // (mosi, miso, sclk)
@@ -69,16 +67,9 @@
     ObjectArea=0; 
     SteeringError=0; 
     DistanceError=0;
-    
-    // Reset all storage
-    xRAvg=0;
-    yRAvg=0; 
-    ObjectWidthAvg=0; 
-    ObjectHeightAvg=0; 
-    ObjectAreaAvg=0; 
 
     CameraId = osThreadCreate(osThread(CameraThread), NULL);
-    CameraPeriodicInt.attach(&CameraPeriodicInterruptISR, 0.05); // 500ms sampling rate    
+    CameraPeriodicInt.attach(&CameraPeriodicInterruptISR, 0.5); // 500ms sampling rate    
 }
 
 
@@ -90,7 +81,6 @@
 *******************************************************************************/
 void CameraThread(void const *argument)
 {
-    static int count = 0;
     
     while (true) 
     {
@@ -101,45 +91,28 @@
         if (blocksR) {
             // signature 0 is cloudy day
             // signature 1 is indoor lighting
-             
+            
+            cameraData_mutex.lock(); 
             xR = pixyR.blocks[SIGNATURE_IN_USE].x;
             yR = pixyR.blocks[SIGNATURE_IN_USE].y;
             ObjectWidth = pixyR.blocks[SIGNATURE_IN_USE].width;
             ObjectHeight = pixyR.blocks[SIGNATURE_IN_USE].height;
             ObjectArea = ObjectHeight * ObjectWidth;
             
-            // Accumulate readings to be used for average value calculation
-            xRAvg += xR;
-            yRAvg += yR; 
-            ObjectWidthAvg += ObjectWidth; 
-            ObjectHeightAvg += ObjectHeight; 
-            ObjectAreaAvg += ObjectArea; 
             
-            count++;
-            // Average calculation 10 readings
-            if(count > 10)
-            {   
-                xRAvg=xRAvg/10;
-                yRAvg=yRAvg/10; 
-                ObjectWidthAvg=ObjectWidthAvg/10; 
-                ObjectHeightAvg=ObjectHeightAvg/10; 
-                ObjectAreaAvg=ObjectAreaAvg/10; 
-            
-                cameraData_mutex.lock();
-                DistanceError = DISTANCE - ObjectWidthAvg;
-                SteeringError = CENTER - xRAvg;
-                cameraData_mutex.unlock();
-                
-                count = 0;
-                    // Reset all storage
-    xRAvg=0;
-    yRAvg=0; 
-    ObjectWidthAvg=0; 
-    ObjectHeightAvg=0; 
-    ObjectAreaAvg=0;
-            }
+            DistanceError = DISTANCE - ObjectWidth;
+            SteeringError = CENTER - xR;
+            cameraData_mutex.unlock();
             
         }
+        // ball not found stop robot from moving
+        else
+        {
+           // set Distance error = 0
+           DistanceError = 0;
+           SteeringError = 0;
+        }
+           
            
     }
     
--- a/CameraThread.h	Sat Mar 03 02:14:40 2018 +0000
+++ b/CameraThread.h	Sat Mar 03 04:47:26 2018 +0000
@@ -10,6 +10,12 @@
 #ifndef CAMERA_INT_H
 #define CAMERA_INT_H
 
+extern int SteeringError, DistanceError;
+extern Mutex cameraData_mutex;
+extern int xR, yR, ObjectWidth, ObjectHeight, ObjectArea, SteeringError, DistanceError;
+extern int xRAvg, yRAvg, ObjectWidthAvg, ObjectHeightAvg, ObjectAreaAvg;
+
+
 void CameraThreadInit(void);
 
 
--- a/Drivers/PiController.cpp	Sat Mar 03 02:14:40 2018 +0000
+++ b/Drivers/PiController.cpp	Sat Mar 03 04:47:26 2018 +0000
@@ -14,10 +14,9 @@
 
 #include "mbed.h"
 #include "PiController.h"
+#include "ui.h"
 
 // global speed variable;
-extern Mutex setpointR_mutex;
-extern Mutex setpointL_mutex;
 float Ki;
 float Kp;
 int32_t scale;
--- a/PiControlThread.cpp	Sat Mar 03 02:14:40 2018 +0000
+++ b/PiControlThread.cpp	Sat Mar 03 04:47:26 2018 +0000
@@ -20,10 +20,11 @@
 #include "Drivers/DE0_driver.h"
 #include "PiControlThread.h"
 #include "Drivers/PiController.h"
+#include "ui.h"
+#include "CameraThread.h"
 
 // setpoints
-extern int setpointR, setpointL;
-extern int SteeringError, DistanceError;
+
 
 // 
 int velR, velL;
@@ -39,10 +40,10 @@
 void PeriodicInterruptISR(void);
 
 // steering gain
-int Ks = 2;
+int Ks = 0.5;
 
 // distance gain
-int Kd = 2;
+int Kd = 4;
 
 // overall robot required speed
 int Setpoint;
@@ -105,8 +106,9 @@
         // Get incremental position and time from QEI
         DE0_read(&sensors);
         
-        SaturateValue(sensors.dp_right, 560);
-        SaturateValue(sensors.dp_left, 560);
+        // might not be needed
+        sensors.dp_right = SaturateValue(sensors.dp_right, 560);
+        sensors.dp_left = SaturateValue(sensors.dp_left, 560);
         
         // Maximum velocity at dPostition = 560 is vel = 703
         velR = (float)((6135.92 * sensors.dp_right) / sensors.dt_right) ;
@@ -123,10 +125,12 @@
         // If distance increase then speed should increase.
         
         // If object is moving away from the the robot increase robot speed
+        cameraData_mutex.lock();
         if(DistanceError > 0)
         {
             // Proportional controller
             Setpoint = (Kd*DistanceError);
+            Setpoint = SaturateValue(Setpoint,560);
         }
         // If object is at the set distance limit or less then do not move.
         else if(DistanceError <= 0)
@@ -137,6 +141,7 @@
         // Decoupled drive  
         setpointR = Setpoint + (Ks*SteeringError);
         setpointL = Setpoint - (Ks*SteeringError);
+        cameraData_mutex.unlock();
         
         // Make sure our limit is not exceeded
         setpointR = SaturateValue(setpointR, 560);
--- a/PiControlThread.h	Sat Mar 03 02:14:40 2018 +0000
+++ b/PiControlThread.h	Sat Mar 03 04:47:26 2018 +0000
@@ -9,6 +9,7 @@
 #ifndef PERIODIC_INT_H
 #define PERIODIC_INT_H
 
+extern osThreadId PiControlId;
 
 void PiControlThreadInit(void);
 
--- a/main.cpp	Sat Mar 03 02:14:40 2018 +0000
+++ b/main.cpp	Sat Mar 03 04:47:26 2018 +0000
@@ -12,6 +12,8 @@
 #include "ExternalInterruptThread.h"
 #include "ui.h"
 #include "CameraThread.h"
+#include "motor_driver_l.h"
+#include "motor_driver_r.h"
 
 /******************************************************************************/
 
@@ -34,6 +36,14 @@
 
     while(1)
     {
+        // if robot goes crazy press 'r' to kill it
+        if(killRobot == true)
+        {
+            osThreadTerminate(PiControlId);
+            motorDriver_L_stop();
+            motorDriver_R_stop(); 
+        }
+        
         consoleUI(); 
         Thread::wait(500); // Go to sleep for 500 ms  
     }
--- a/ui.cpp	Sat Mar 03 02:14:40 2018 +0000
+++ b/ui.cpp	Sat Mar 03 04:47:26 2018 +0000
@@ -13,6 +13,7 @@
 #include "mbed.h"
 #include "WatchdogThread.h"
 #include "ui.h"
+#include "CameraThread.h"
 
 Serial bluetooth(p9,p10); // Define the bluetooth channel and IO pins
 Serial pc(USBTX, USBRX); // Pins (tx, rx) for PC serial channel
@@ -24,23 +25,11 @@
 int setpointR = 0;
 int setpointL = 0;
 
+bool killRobot = false;
+
 // variable to store character recieved from terminal
 char x;
 
-/*
-extern int16_t dPosition, dTime;
-extern float Ki; 
-extern float Kp;
-extern int vel;
-extern int32_t e;
-extern int32_t xState;
-extern int32_t u;
-//extern int time_passed;
-*/
-
-extern int xR, yR, ObjectWidth, ObjectHeight, ObjectArea, SteeringError, DistanceError;
-extern int xRAvg, yRAvg, ObjectWidthAvg, ObjectHeightAvg, ObjectAreaAvg;
-extern Mutex cameraData_mutex;
 int16_t position;
 
 /******************************************************************************
@@ -48,8 +37,24 @@
 ******************************************************************************/
 void consoleUI(void){
 
-    //bluetooth.printf("\r\nsetpointR: %d   setpointL: %d    ObjectWidth: %d     ObjectXcoordinate: %d   SteeringError: %d   DistanceError: %d", setpointR, setpointL, ObjectWidth, xR, SteeringError, DistanceError);    
-    bluetooth.printf("\r\nsetpointR: %d   setpointL: %d    ObjectWidth: %d     ObjectXcoordinate: %d   SteeringError: %d   DistanceError: %d", setpointR, setpointL, ObjectWidth, xR, SteeringError, DistanceError);       
+    cameraData_mutex.lock();
+    bluetooth.printf("\r\nsetpointR: %d   setpointL: %d    ObjectWidth: %d     ObjectXcoordinate: %d   SteeringError: %d   DistanceError: %d", setpointR, setpointL, ObjectWidth, xR, SteeringError, DistanceError);    
+    cameraData_mutex.unlock();
+    //bluetooth.printf("\r\nsetpointR: %d   setpointL: %d    ObjectWidth: %d     ObjectXcoordinate: %d   SteeringError: %d   DistanceError: %d", setpointR, setpointL, ObjectWidth, xR, SteeringError, DistanceError);       
+
+    // safety mechanism
+    if (bluetooth.readable()){
+        x = bluetooth.getc();
+        if (x == 'r')
+        {
+            killRobot = true;
+        }
+        else if (x == 's')
+        {
+            killRobot = false;
+        }
+    }
+    
 }
 
 
@@ -75,7 +80,7 @@
 /******************************************************************************
             A function to Display startup Messsage
 ******************************************************************************/
-void displayStartupMsg()
+void displayStartupMsg_old()
 {
     bluetooth.printf("\r\n************************************");
     bluetooth.printf("\r\n**** DC Motor Control using PWM ****");
@@ -87,6 +92,13 @@
     bluetooth.printf("\r\n-press k to decrease motor speedL");
 }
 
+void displayStartupMsg()
+{
+    bluetooth.printf("\r\n************************************");
+    bluetooth.printf("\r\n**** AUTONOMOUS FOLLOWING ROBOT ****");
+    bluetooth.printf("\r\n************************************");
+    bluetooth.printf("\r\n-PRESS 'r' TO KILL ROBOT");
+}
 
 /******************************************************************************
                            User interface 1
--- a/ui.h	Sat Mar 03 02:14:40 2018 +0000
+++ b/ui.h	Sat Mar 03 04:47:26 2018 +0000
@@ -11,6 +11,13 @@
 
 #define SPEED_STEP     1
 
+extern bool killRobot;
+extern int setpointR;
+extern int setpointL;
+extern Serial bluetooth;
+extern Mutex setpointR_mutex;
+extern Mutex setpointL_mutex;
+
 void consoleUI(void);
 void displayStartupMsg(void);
 void twoTerminalsTest(void);