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:
Fri Mar 02 23:37:31 2018 +0000
Parent:
9:fe56b888985c
Child:
11:9135e5bc2fcf
Commit message:
vision + robot testing

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
PiControlThread.cpp Show annotated file Show diff for this revision Revisions of this file
TPixy-Interface.lib 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
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/CameraThread.cpp	Fri Mar 02 23:37:31 2018 +0000
@@ -0,0 +1,116 @@
+/******************************************************************************/
+// ECE4333
+// LAB Partner 1:   Ahmed Sobhy - ID: 3594449
+// LAB Partner 2:   Brandon Kingman - ID: 3470444
+// Project:         Autonomous Robot Design
+// Instructor:      Prof. Chris Diduch
+/******************************************************************************/
+// filename: CameraThread.cpp
+// file content description:
+//      * Function to Create the Camera Thread
+//      * CameraThread
+//      * CameraThread ISR
+//      * Variables related to the functionality of the thread
+/******************************************************************************/
+
+#include "mbed.h"
+#include "Pixy.h"
+
+#define CENTER              160
+#define DISTANCE            20
+#define SIGNATURE_IN_USE    0
+
+osThreadId CameraId;
+
+extern Serial bluetooth;
+
+SPI spiR(p11, p12, p13); // (mosi, miso, sclk)
+PixySPI pixyR(&spiR, &bluetooth);
+
+void CameraThread(void const *argument);
+void CameraPeriodicInterruptISR(void);
+
+/******************************************************************************/
+//  osPriorityIdle          = -3,          ///< priority: idle (lowest)
+//  osPriorityLow           = -2,          ///< priority: low
+//  osPriorityBelowNormal   = -1,          ///< priority: below normal
+//  osPriorityNormal        =  0,          ///< priority: normal (default)
+//  osPriorityAboveNormal   = +1,          ///< priority: above normal
+//  osPriorityHigh          = +2,          ///< priority: high
+//  osPriorityRealtime      = +3,          ///< priority: realtime (highest)
+/******************************************************************************/
+
+// Declare PeriodicInterruptThread as a thread/process
+osThreadDef(CameraThread, osPriorityHigh, 1024); 
+
+Ticker CameraPeriodicInt;      // Declare a timer interrupt: PeriodicInt
+
+int xR, yR, ObjectWidth, ObjectHeight, ObjectArea, SteeringError, DistanceError;
+uint16_t blocksR;
+
+/*******************************************************************************
+* @brief    function that creates a thread for the Camera
+* @param    none
+* @return   none
+*******************************************************************************/
+void CameraThreadInit()
+{
+    pixyR.init();
+    xR=0;
+    yR=0;
+    ObjectWidth=0;
+    ObjectHeight=0; 
+    ObjectArea=0; 
+    SteeringError=0; 
+    DistanceError=0;
+    
+    CameraId = osThreadCreate(osThread(CameraThread), NULL);
+    CameraPeriodicInt.attach(&CameraPeriodicInterruptISR, 0.02);   // 20ms sampling rate - 50fps    
+}
+
+
+/*******************************************************************************
+* @brief    This is the Camera thread. It reads several values from the 
+*           Camera: x coordinate error from center @ 160 & the block area of the object
+* @param    none
+* @return   none
+*******************************************************************************/
+void CameraThread(void const *argument)
+{
+    
+    while (true) 
+    {
+        
+        osSignalWait(0x01, osWaitForever); // Go to sleep until signal is received.
+        
+        blocksR = pixyR.getBlocks();
+        if (blocksR) {
+            // signature 0 is cloudy day
+            // signature 1 is indoor lighting
+            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;
+            
+            DistanceError = DISTANCE - ObjectWidth;
+            SteeringError = CENTER - xR;
+            
+        }
+           
+    }
+    
+}
+
+/*******************************************************************************
+* @brief    The ISR below signals the CameraThread. it is setup to run 
+*           every 20ms
+* @param    none
+* @return   none
+*******************************************************************************/
+void CameraPeriodicInterruptISR(void)
+{
+    // Send signal to the thread with ID, PeriodicInterruptId, i.e., PeriodicInterruptThread.
+    osSignalSet(CameraId,0x1); 
+}
+
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/CameraThread.h	Fri Mar 02 23:37:31 2018 +0000
@@ -0,0 +1,18 @@
+
+/******************************************************************************/
+// ECE4333
+// LAB Partner 1:   Ahmed Sobhy - ID: 3594449
+// LAB Partner 2:   Brandon Kingman - ID: 3470444
+// Project:         Autonomous Robot Design
+// Instructor:      Prof. Chris Diduch
+/******************************************************************************/
+
+#ifndef CAMERA_INT_H
+#define CAMERA_INT_H
+
+void CameraThreadInit(void);
+
+
+#endif
+
+
--- a/PiControlThread.cpp	Fri Feb 23 20:58:34 2018 +0000
+++ b/PiControlThread.cpp	Fri Mar 02 23:37:31 2018 +0000
@@ -21,9 +21,14 @@
 #include "PiControlThread.h"
 #include "Drivers/PiController.h"
 
+// setpoints
 extern int setpointR, setpointL;
+extern int SteeringError, DistanceError;
 
+// 
 int velR, velL;
+
+// control signal
 int32_t U_right, U_left;
 
 sensors_t sensors;
@@ -33,6 +38,15 @@
 void PiControlThread(void const *);
 void PeriodicInterruptISR(void);
 
+// steering gain
+int Ks = 1;
+
+// distance gain
+int Kd = 1;
+
+// overall robot required speed
+int Setpoint;
+
 osThreadId PiControlId;
 
 /******************************************************************************/
@@ -76,6 +90,10 @@
 }
 
 
+
+
+
+
 /*******************************************************************************
 * @brief    This is the PI controller thread. It reads several values from the 
 *           FPGA such as speed, time and other sensors data
@@ -89,8 +107,6 @@
     {
         osSignalWait(0x01, osWaitForever); // Go to sleep until signal, SignalPi, is received.
         
-        //time_passed++;
-        
         // get incremental position and time from QEI
         DE0_read(&sensors);
         
@@ -103,6 +119,27 @@
         // maximum velocity at dPostition = 560 is vel = 703
         velL = (float)((6135.92 * sensors.dp_left) / sensors.dt_left) ;
         
+        /*********************Differential Start*******************************/
+        // Inputs are Speed Setpoint and Steering Setpoint
+        // The Inputs for the Steering are specified in the CameraThread
+        // and set at the center.
+        // The distance between the object and the image should be set to 1 meter   
+        // If distance decrease then speed should stop.
+        // If distance increase then speed should increase.
+        
+        // if object is moving away from the the robot increase robot speed
+        if(DistanceError > 0)
+        {
+            Setpoint = (Kd*DistanceError);
+        }
+        // if object is at the set distance limit or less then do not move.
+        else if(DistanceError <= 0)
+        {
+            Setpoint = 0;
+        }
+        
+        setpointR = Setpoint + (Ks*SteeringError);
+        setpointL = Setpoint - (Ks*SteeringError);
         
         U_right = PiControllerR(setpointR,sensors.dp_right);
         U_left  = PiControllerL(setpointL,sensors.dp_left);
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/TPixy-Interface.lib	Fri Mar 02 23:37:31 2018 +0000
@@ -0,0 +1,1 @@
+https://os.mbed.com/users/swilkins8/code/TPixy-Interface/#66df7d295245
--- a/main.cpp	Fri Feb 23 20:58:34 2018 +0000
+++ b/main.cpp	Fri Mar 02 23:37:31 2018 +0000
@@ -11,6 +11,7 @@
 #include "PiControlThread.h"
 #include "ExternalInterruptThread.h"
 #include "ui.h"
+#include "CameraThread.h"
 
 /******************************************************************************/
 
@@ -27,12 +28,13 @@
 
     // Initialize and run the threads below:
     WatchdogThreadInit();
+    CameraThreadInit();
     PiControlThreadInit();
     ExternalInterruptThreadInit();   
 
     while(1)
     {
-        consoleUI(); 
+        //consoleUI(); 
         Thread::wait(500); // Go to sleep for 500 ms  
     }
 }