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 Feb 23 20:58:34 2018 +0000
Parent:
8:a0890fa79084
Child:
10:8919b1b76243
Commit message:
right after the two motors are running

Changed in this revision

Drivers/DE0_driver.cpp Show annotated file Show diff for this revision Revisions of this file
Drivers/DE0_driver.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
Drivers/PiController.h Show annotated file Show diff for this revision Revisions of this file
Drivers/motor_driver.cpp Show diff for this revision Revisions of this file
Drivers/motor_driver.h Show diff for this revision Revisions of this file
Drivers/motor_driver_l.cpp Show annotated file Show diff for this revision Revisions of this file
Drivers/motor_driver_l.h Show annotated file Show diff for this revision Revisions of this file
Drivers/motor_driver_r.cpp Show annotated file Show diff for this revision Revisions of this file
Drivers/motor_driver_r.h Show annotated file Show diff for this revision Revisions of this file
ExternalInterruptThread.cpp Show annotated file Show diff for this revision Revisions of this file
ExternalInterruptThread.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
PiControlThread.h Show annotated file Show diff for this revision Revisions of this file
WatchdogThread.cpp Show annotated file Show diff for this revision Revisions of this file
WatchdogThread.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/Drivers/DE0_driver.cpp	Mon Feb 19 17:42:33 2018 +0000
+++ b/Drivers/DE0_driver.cpp	Fri Feb 23 20:58:34 2018 +0000
@@ -1,9 +1,9 @@
 /******************************************************************************/
 // ECE4333
-// LAB Partner 1: Ahmed Sobhy - ID: 3594449
-// LAB Partner 2: Brandon Kingman - ID: 3470444
-// Project: Autonomous Robot Design
-// Instructor: Prof. Chris Diduch
+// LAB Partner 1:   Ahmed Sobhy - ID: 3594449
+// LAB Partner 2:   Brandon Kingman - ID: 3470444
+// Project:         Autonomous Robot Design
+// Instructor:      Prof. Chris Diduch
 /******************************************************************************/
 // filename: DE0_driver.cpp
 // file content description:
@@ -13,6 +13,7 @@
 /******************************************************************************/
 
 #include "mbed.h"
+#include "DE0_driver.h"
 
 #define DUMMY   0
 
@@ -24,7 +25,6 @@
 DigitalOut IoReset(p15);    // 0-1-0 reset for all SPI peripherals in the DE0 FPGA
 DigitalOut SpiReset(p14);   // 0-1-0 tells the DE0 SPI interace that the next word sent is a control word
 
-
 int SignExtend(int16_t x)
 {
     // if number is negative
@@ -100,17 +100,21 @@
 * @param    none
 * @return   none
 *******************************************************************************/
-void DE0_read(uint16_t * id, int * dP, uint16_t * dT)
+void DE0_read(sensors_t * sensors)
 {
     // To place SPI module into control mode, where the next word received by the
     // slave is interpreted as a control word.
     RestartSpi();
     
     // rd = 1 (control if 1) address = 0 no of word = 2 - Specify a read only transactions of 2 words
-    *id = (uint16_t)DE0.write(0x8002);
+    sensors->id = (uint16_t)DE0.write(0x8004);
     
     // Read the two 16 bits registers from the FPGA 
-    *dP = SignExtend(DE0.write(DUMMY)); // A SPI read only transaction occurs.
-    *dT = DE0.write(DUMMY); //
+    sensors->dp_right = SignExtend(DE0.write(DUMMY)); // A SPI read only transaction occurs.
+    sensors->dt_right = DE0.write(DUMMY); //
+    
+    sensors->dp_left = SignExtend(DE0.write(DUMMY)); // A SPI read only transaction occurs.
+    sensors->dt_left = DE0.write(DUMMY); //
+    
 }
     
\ No newline at end of file
--- a/Drivers/DE0_driver.h	Mon Feb 19 17:42:33 2018 +0000
+++ b/Drivers/DE0_driver.h	Fri Feb 23 20:58:34 2018 +0000
@@ -1,19 +1,25 @@
 /******************************************************************************/
 // ECE4333
-// LAB Partner 1: Ahmed Sobhy - ID: 3594449
-// LAB Partner 2: Brandon Kingman - ID: 3470444
-// Project: Autonomous Robot Design
-// Instructor: Prof. Chris Diduch
+// LAB Partner 1:   Ahmed Sobhy - ID: 3594449
+// LAB Partner 2:   Brandon Kingman - ID: 3470444
+// Project:         Autonomous Robot Design
+// Instructor:      Prof. Chris Diduch
 /******************************************************************************/
 
 #ifndef DE0_DRIVER_H
 #define DE0_DRIVER_H
 
-
+typedef struct {
+    uint16_t id;
+    int dp_right;
+    uint16_t dt_right;
+    int dp_left;
+    uint16_t dt_left;
+    } sensors_t;
 
 
 void DE0_init(void);
-void DE0_read(uint16_t *, int * , uint16_t *);
+void DE0_read(sensors_t * sensors);
 
 
 
--- a/Drivers/PiController.cpp	Mon Feb 19 17:42:33 2018 +0000
+++ b/Drivers/PiController.cpp	Fri Feb 23 20:58:34 2018 +0000
@@ -1,9 +1,9 @@
 /******************************************************************************/
 // ECE4333
-// LAB Partner 1: Ahmed Sobhy - ID: 3594449
-// LAB Partner 2: Brandon Kingman - ID: 3470444
-// Project: Autonomous Robot Design
-// Instructor: Prof. Chris Diduch
+// LAB Partner 1:   Ahmed Sobhy - ID: 3594449
+// LAB Partner 2:   Brandon Kingman - ID: 3470444
+// Project:         Autonomous Robot Design
+// Instructor:      Prof. Chris Diduch
 /******************************************************************************/
 // filename: PiController.cpp
 // file content description:
@@ -16,10 +16,11 @@
 #include "PiController.h"
 
 // global speed variable;
-extern Mutex setpoint_mutex;
+extern Mutex setpointR_mutex;
+extern Mutex setpointL_mutex;
 float Ki;
 float Kp;
-int32_t e, u, xState, scale;
+int32_t scale;
 
 /******************************************************************************/
 int SaturatingSubtract(int x, int y)
@@ -58,7 +59,7 @@
     
     // initialization
     scale = 40;
-    xState = 0;  
+    //xState = 0;  
         
 }
 
@@ -70,17 +71,20 @@
 * @return   u is the control signal out of the controller to make the system 
 *           reach the desired setpoint
 *******************************************************************************/
-uint32_t PiController(int setp, int dP)
+uint32_t PiControllerR(int setp, int dP)
 {
-    
+    static int32_t e, u; 
+    static int32_t xState = 0;
     int32_t xTemp;
     int32_t uProportional;
     int32_t uIntegral;
     int32_t uS;
     
-    setpoint_mutex.lock();
+    setpointR_mutex.lock();
+    setpointL_mutex.lock();
     e = SaturatingSubtract(setp, dP);  // e is the velocity error
-    setpoint_mutex.unlock();
+    setpointL_mutex.unlock();
+    setpointR_mutex.unlock();
 
     xTemp = SaturatingAdd(xState, e);
 
@@ -100,3 +104,45 @@
     return u;
     
 }
+
+
+/*******************************************************************************
+* @brief    PI Controller function
+* @param    setp is the setpoint we would like the system to get to
+* @param    dP is the current speed of the system 
+* @return   u is the control signal out of the controller to make the system 
+*           reach the desired setpoint
+*******************************************************************************/
+uint32_t PiControllerL(int setp, int dP)
+{
+    static int32_t e, u;
+    static int32_t xState = 0;
+    int32_t xTemp;
+    int32_t uProportional;
+    int32_t uIntegral;
+    int32_t uS;
+    
+    setpointR_mutex.lock();
+    setpointL_mutex.lock();
+    e = SaturatingSubtract(setp, dP);  // e is the velocity error
+    setpointL_mutex.unlock();
+    setpointR_mutex.unlock();
+
+    xTemp = SaturatingAdd(xState, e);
+
+    // the maximum value that 'u' can get to is 20
+    // the maximum value that dPosition can get to 560
+    // scaling factor is 560/20 = 28
+    // scaling factor used is 40
+    
+    uProportional = (float)(Kp*e/scale);
+    uIntegral = (float)(Ki*xState/scale);
+
+    uS = SaturatingAdd(uProportional, uIntegral);
+
+    u = SaturateValue(uS, U_LIMIT);
+    if(u==uS) xState=xTemp; // if limit has not been reached then update xState
+    
+    return u;
+    
+}
--- a/Drivers/PiController.h	Mon Feb 19 17:42:33 2018 +0000
+++ b/Drivers/PiController.h	Fri Feb 23 20:58:34 2018 +0000
@@ -1,9 +1,9 @@
 /******************************************************************************/
 // ECE4333
-// LAB Partner 1: Ahmed Sobhy - ID: 3594449
-// LAB Partner 2: Brandon Kingman - ID: 3470444
-// Project: Autonomous Robot Design
-// Instructor: Prof. Chris Diduch
+// LAB Partner 1:   Ahmed Sobhy - ID: 3594449
+// LAB Partner 2:   Brandon Kingman - ID: 3470444
+// Project:         Autonomous Robot Design
+// Instructor:      Prof. Chris Diduch
 /******************************************************************************/
 
 #ifndef PICONTROLLER_H
@@ -15,7 +15,8 @@
 int SaturatingSubtract(int x, int y);
 int SaturatingAdd(int x, int y);
 void PiController_init(float,float);
-uint32_t PiController(int,int);
+uint32_t PiControllerR(int,int);
+uint32_t PiControllerL(int,int);
 
 
 #endif
\ No newline at end of file
--- a/Drivers/motor_driver.cpp	Mon Feb 19 17:42:33 2018 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,109 +0,0 @@
-/******************************************************************************/
-// ECE4333
-// LAB Partner 1: Ahmed Sobhy - ID: 3594449
-// LAB Partner 2: Brandon Kingman - ID: 3470444
-// Project: Autonomous Robot Design
-// Instructor: Prof. Chris Diduch
-/******************************************************************************/
-// filename: motor_driver.cpp
-// file content description:
-//      * function to initialize the hardware used to control a DC Motor
-//      * functions to drive to motor in forward, reverse and stop.
-/******************************************************************************/
-
-#include "mbed.h"
-#include "motor_driver.h"
-
-
-
-DigitalOut DirR(p29);    // Right motor direction control pin
-PwmOut PwmR(p21);       // Right motor PWM
-
-Mutex SpeedMutex;
-
-motor_state_t motor_status = MOTOR_UNINIT;
-
-
-/*******************************************************************************
-* @brief  This function initializes the PWM channel to be used to control a DC
-*         motor
-* @param  none
-* @return motor_state_t the state of the motor driver
-*******************************************************************************/
-motor_state_t motorDriver_init()
-{
-    // motor is still uninitialized
-    motor_status = MOTOR_UNINIT;
-
-    PwmR.period_us(PERIOD); // This sets the PWM period in us
-    // DC Motor Control Pins
-    DirR = 0;
-    PwmR = 0;
-
-    motor_status = MOTOR_INIT;
-
-    return motor_status;
-
-}
-
-/*******************************************************************************
-* @brief  This function sets the speed and the direction of the DC motor to move
-*         forward
-* @param  speed
-* @return motor_state_t the state of the motor driver
-*******************************************************************************/
-motor_state_t motorDriver_forward(int speed)
-{
-
-    if( motor_status != MOTOR_UNINIT ) 
-    {
-        SpeedMutex.lock();
-        DirR = 0;
-        PwmR.pulsewidth_us(abs(speed));
-        SpeedMutex.unlock();
-        motor_status = MOTOR_FORWARD;
-    }
-
-    return motor_status;
-
-}
-
-/*******************************************************************************
-* @brief  This function sets the speed and the direction of the DC motor to move
-*         backwards
-* @param  speed
-* @return motor_state_t the state of the motor driver
-*******************************************************************************/
-motor_state_t motorDriver_reverse(int speed)
-{
-
-    if( motor_status != MOTOR_UNINIT ) 
-    {
-        SpeedMutex.lock();
-        DirR = 1;
-        PwmR.pulsewidth_us(abs(speed));
-        SpeedMutex.unlock();
-        motor_status = MOTOR_REVERSE;
-    }
-
-    return motor_status;
-
-}
-
-/*******************************************************************************
-* @brief  This function stops the motor
-* @param  none
-* @return motor_state_t the state of the motor driver
-*******************************************************************************/
-motor_state_t motorDriver_stop()
-{
-    
-    if( motor_status != MOTOR_UNINIT ) 
-    {
-        PwmR.pulsewidth_us(0);
-        motor_status = MOTOR_STOPPED;
-    }
-
-    return motor_status;
-
-}
--- a/Drivers/motor_driver.h	Mon Feb 19 17:42:33 2018 +0000
+++ /dev/null	Thu Jan 01 00:00:00 1970 +0000
@@ -1,27 +0,0 @@
-/******************************************************************************/
-// ECE4333
-// LAB Partner 1: Ahmed Sobhy - ID: 3594449
-// LAB Partner 2: Brandon Kingman - ID: 3470444
-// Project: Autonomous Robot Design
-// Instructor: Prof. Chris Diduch
-/******************************************************************************/
-
-#ifndef MOTOR_DRIVER_H
-#define MOTOR_DRIVER_H
-
-//motor configuration
-#define PERIOD  40     // period in us -> frequency = 25kHz -- audible frequency is between 20Hz - 20kHz
-
-typedef enum { MOTOR_UNINIT,
-               MOTOR_INIT,
-               MOTOR_FORWARD,
-               MOTOR_REVERSE,
-               MOTOR_STOPPED
-             } motor_state_t;
-
-motor_state_t motorDriver_init();
-motor_state_t motorDriver_forward(int);
-motor_state_t motorDriver_reverse(int);
-motor_state_t motorDriver_stop();
-
-#endif
\ No newline at end of file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Drivers/motor_driver_l.cpp	Fri Feb 23 20:58:34 2018 +0000
@@ -0,0 +1,112 @@
+/******************************************************************************/
+// ECE4333
+// LAB Partner 1:   Ahmed Sobhy - ID: 3594449
+// LAB Partner 2:   Brandon Kingman - ID: 3470444
+// Project:         Autonomous Robot Design
+// Instructor:      Prof. Chris Diduch
+/******************************************************************************/
+// filename: motor_driver.cpp
+// file content description:
+//      * function to initialize the hardware used to control a DC Motor
+//      * functions to drive to motor in forward, reverse and stop.
+/******************************************************************************/
+
+#include "mbed.h"
+#include "motor_driver_r.h"
+#include "motor_driver_l.h"
+
+
+
+DigitalOut DirL(p30);    // Right motor direction control pin
+PwmOut PwmL(p22);       // Right motor PWMz
+
+Mutex SpeedMutexL;
+
+static motor_state_t motor_status = MOTOR_UNINIT;
+
+
+/*******************************************************************************
+* @brief  This function initializes the PWM channel to be used to control a DC
+*         motor
+* @param  none
+* @return motor_state_t the state of the motor driver
+*******************************************************************************/
+motor_state_t motorDriver_L_init()
+{
+    // motor is still uninitialized
+    motor_status = MOTOR_UNINIT;
+
+    // DC Motor Control Pins
+    DirL = 0;
+    PwmL = 0;
+
+    PwmL.period_us(PERIOD_L); // This sets the PWM period in us
+
+
+    motor_status = MOTOR_INIT;
+
+    return motor_status;
+
+}
+
+/*******************************************************************************
+* @brief  This function sets the speed and the direction of the DC motor to move
+*         forward
+* @param  speed
+* @return motor_state_t the state of the motor driver
+*******************************************************************************/
+motor_state_t motorDriver_L_forward(int speed)
+{
+
+    if( motor_status != MOTOR_UNINIT ) 
+    {
+        SpeedMutexL.lock();
+        DirL = 0;
+        PwmL.pulsewidth_us(abs(speed));
+        SpeedMutexL.unlock();
+        motor_status = MOTOR_FORWARD;
+    }
+
+    return motor_status;
+
+}
+
+/*******************************************************************************
+* @brief  This function sets the speed and the direction of the DC motor to move
+*         backwards
+* @param  speed
+* @return motor_state_t the state of the motor driver
+*******************************************************************************/
+motor_state_t motorDriver_L_reverse(int speed)
+{
+
+    if( motor_status != MOTOR_UNINIT ) 
+    {
+        SpeedMutexL.lock();
+        DirL = 1;
+        PwmL.pulsewidth_us(abs(speed));
+        SpeedMutexL.unlock();
+        motor_status = MOTOR_REVERSE;
+    }
+
+    return motor_status;
+
+}
+
+/*******************************************************************************
+* @brief  This function stops the motor
+* @param  none
+* @return motor_state_t the state of the motor driver
+*******************************************************************************/
+motor_state_t motorDriver_L_stop()
+{
+    
+    if( motor_status != MOTOR_UNINIT ) 
+    {
+        PwmL.pulsewidth_us(0);
+        motor_status = MOTOR_STOPPED;
+    }
+
+    return motor_status;
+
+}
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Drivers/motor_driver_l.h	Fri Feb 23 20:58:34 2018 +0000
@@ -0,0 +1,28 @@
+/******************************************************************************/
+// ECE4333
+// LAB Partner 1:   Ahmed Sobhy - ID: 3594449
+// LAB Partner 2:   Brandon Kingman - ID: 3470444
+// Project:         Autonomous Robot Design
+// Instructor:      Prof. Chris Diduch
+/******************************************************************************/
+
+#ifndef MOTOR_DRIVER_L_H
+#define MOTOR_DRIVER_L_H
+
+#include "motor_driver_r.h"
+//motor configuration
+#define PERIOD_L  40     // period in us -> frequency = 25kHz -- audible frequency is between 20Hz - 20kHz
+/*
+typedef enum { MOTOR_UNINIT,
+               MOTOR_INIT,
+               MOTOR_FORWARD,
+               MOTOR_REVERSE,
+               MOTOR_STOPPED
+             } motor_state_t;
+*/
+motor_state_t motorDriver_L_init();
+motor_state_t motorDriver_L_forward(int);
+motor_state_t motorDriver_L_reverse(int);
+motor_state_t motorDriver_L_stop();
+
+#endif
\ No newline at end of file
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Drivers/motor_driver_r.cpp	Fri Feb 23 20:58:34 2018 +0000
@@ -0,0 +1,111 @@
+/******************************************************************************/
+// ECE4333
+// LAB Partner 1:   Ahmed Sobhy - ID: 3594449
+// LAB Partner 2:   Brandon Kingman - ID: 3470444
+// Project:         Autonomous Robot Design
+// Instructor:      Prof. Chris Diduch
+/******************************************************************************/
+// filename: motor_driver.cpp
+// file content description:
+//      * function to initialize the hardware used to control a DC Motor
+//      * functions to drive to motor in forward, reverse and stop.
+/******************************************************************************/
+
+#include "mbed.h"
+#include "motor_driver_r.h"
+
+
+
+DigitalOut DirR(p29);    // Right motor direction control pin
+PwmOut PwmR(p21);       // Right motor PWMz
+
+Mutex SpeedMutexR;
+
+static motor_state_t motor_status = MOTOR_UNINIT;
+
+
+/*******************************************************************************
+* @brief  This function initializes the PWM channel to be used to control a DC
+*         motor
+* @param  none
+* @return motor_state_t the state of the motor driver
+*******************************************************************************/
+motor_state_t motorDriver_R_init()
+{
+    // motor is still uninitialized
+    motor_status = MOTOR_UNINIT;
+
+    // DC Motor Control Pins
+    DirR = 0;
+    PwmR = 0;
+
+    PwmR.period_us(PERIOD_R); // This sets the PWM period in us
+
+
+    motor_status = MOTOR_INIT;
+
+    return motor_status;
+
+}
+
+/*******************************************************************************
+* @brief  This function sets the speed and the direction of the DC motor to move
+*         forward
+* @param  speed
+* @return motor_state_t the state of the motor driver
+*******************************************************************************/
+motor_state_t motorDriver_R_forward(int speed)
+{
+
+    if( motor_status != MOTOR_UNINIT ) 
+    {
+        SpeedMutexR.lock();
+        DirR = 0;
+        PwmR.pulsewidth_us(abs(speed));
+        SpeedMutexR.unlock();
+        motor_status = MOTOR_FORWARD;
+    }
+
+    return motor_status;
+
+}
+
+/*******************************************************************************
+* @brief  This function sets the speed and the direction of the DC motor to move
+*         backwards
+* @param  speed
+* @return motor_state_t the state of the motor driver
+*******************************************************************************/
+motor_state_t motorDriver_R_reverse(int speed)
+{
+
+    if( motor_status != MOTOR_UNINIT ) 
+    {
+        SpeedMutexR.lock();
+        DirR = 1;
+        PwmR.pulsewidth_us(abs(speed));
+        SpeedMutexR.unlock();
+        motor_status = MOTOR_REVERSE;
+    }
+
+    return motor_status;
+
+}
+
+/*******************************************************************************
+* @brief  This function stops the motor
+* @param  none
+* @return motor_state_t the state of the motor driver
+*******************************************************************************/
+motor_state_t motorDriver_R_stop()
+{
+    
+    if( motor_status != MOTOR_UNINIT ) 
+    {
+        PwmR.pulsewidth_us(0);
+        motor_status = MOTOR_STOPPED;
+    }
+
+    return motor_status;
+
+}
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Drivers/motor_driver_r.h	Fri Feb 23 20:58:34 2018 +0000
@@ -0,0 +1,27 @@
+/******************************************************************************/
+// ECE4333
+// LAB Partner 1:   Ahmed Sobhy - ID: 3594449
+// LAB Partner 2:   Brandon Kingman - ID: 3470444
+// Project:         Autonomous Robot Design
+// Instructor:      Prof. Chris Diduch
+/******************************************************************************/
+
+#ifndef MOTOR_DRIVER_R_H
+#define MOTOR_DRIVER_R_H
+
+//motor configuration
+#define PERIOD_R  40     // period in us -> frequency = 25kHz -- audible frequency is between 20Hz - 20kHz
+
+typedef enum { MOTOR_UNINIT,
+               MOTOR_INIT,
+               MOTOR_FORWARD,
+               MOTOR_REVERSE,
+               MOTOR_STOPPED
+             } motor_state_t;
+
+motor_state_t motorDriver_R_init();
+motor_state_t motorDriver_R_forward(int);
+motor_state_t motorDriver_R_reverse(int);
+motor_state_t motorDriver_R_stop();
+
+#endif
\ No newline at end of file
--- a/ExternalInterruptThread.cpp	Mon Feb 19 17:42:33 2018 +0000
+++ b/ExternalInterruptThread.cpp	Fri Feb 23 20:58:34 2018 +0000
@@ -1,9 +1,9 @@
 /******************************************************************************/
 // ECE4333
-// LAB Partner 1: Ahmed Sobhy - ID: 3594449
-// LAB Partner 2: Brandon Kingman - ID: 3470444
-// Project: Autonomous Robot Design
-// Instructor: Prof. Chris Diduch
+// LAB Partner 1:   Ahmed Sobhy - ID: 3594449
+// LAB Partner 2:   Brandon Kingman - ID: 3470444
+// Project:         Autonomous Robot Design
+// Instructor:      Prof. Chris Diduch
 /******************************************************************************/
 // filename: ExternalInterruptThread.cpp
 // file content description:
@@ -11,6 +11,7 @@
 //      * ExternalInterruptThread
 //      * ExternalInterrupt ISR
 /******************************************************************************/
+
 #include "mbed.h"
 
 void ExtInterruptISR(void);
--- a/ExternalInterruptThread.h	Mon Feb 19 17:42:33 2018 +0000
+++ b/ExternalInterruptThread.h	Fri Feb 23 20:58:34 2018 +0000
@@ -1,9 +1,9 @@
 /******************************************************************************/
 // ECE4333
-// LAB Partner 1: Ahmed Sobhy - ID: 3594449
-// LAB Partner 2: Brandon Kingman - ID: 3470444
-// Project: Autonomous Robot Design
-// Instructor: Prof. Chris Diduch
+// LAB Partner 1:   Ahmed Sobhy - ID: 3594449
+// LAB Partner 2:   Brandon Kingman - ID: 3470444
+// Project:         Autonomous Robot Design
+// Instructor:      Prof. Chris Diduch
 /******************************************************************************/
 
 #ifndef EXTERNAL_INT_H
--- a/PiControlThread.cpp	Mon Feb 19 17:42:33 2018 +0000
+++ b/PiControlThread.cpp	Fri Feb 23 20:58:34 2018 +0000
@@ -1,9 +1,9 @@
 /******************************************************************************/
 // ECE4333
-// LAB Partner 1: Ahmed Sobhy - ID: 3594449
-// LAB Partner 2: Brandon Kingman - ID: 3470444
-// Project: Autonomous Robot Design
-// Instructor: Prof. Chris Diduch
+// LAB Partner 1:   Ahmed Sobhy - ID: 3594449
+// LAB Partner 2:   Brandon Kingman - ID: 3470444
+// Project:         Autonomous Robot Design
+// Instructor:      Prof. Chris Diduch
 /******************************************************************************/
 // filename: PiControlThread.cpp
 // file content description:
@@ -15,17 +15,18 @@
 
 #include "mbed.h"
 #include "ui.h"
-#include "Drivers/motor_driver.h"
+#include "Drivers/motor_driver_r.h"
+#include "Drivers/motor_driver_l.h"
 #include "Drivers/DE0_driver.h"
 #include "PiControlThread.h"
 #include "Drivers/PiController.h"
 
-extern int setpoint;
+extern int setpointR, setpointL;
 
-uint16_t ID, dTime;
-int dPosition;
-int vel;
-int32_t U;
+int velR, velL;
+int32_t U_right, U_left;
+
+sensors_t sensors;
 
 int time_passed = 0;
 
@@ -61,7 +62,8 @@
 void PiControlThreadInit()
 {
     DE0_init();                  // initialize FPGA
-    motorDriver_init();          // initialize motorDriver
+    motorDriver_R_init();          // initialize motorDriver
+    motorDriver_L_init();          // initialize motorDriver
     //                Kp,Ki
     PiController_init(1,0.4); // initialize the PI Controller gains and initialize variables
     
@@ -87,24 +89,42 @@
     {
         osSignalWait(0x01, osWaitForever); // Go to sleep until signal, SignalPi, is received.
         
-        time_passed++;
+        //time_passed++;
         
         // get incremental position and time from QEI
-        DE0_read(&ID, &dPosition, &dTime);
-        SaturateValue(dPosition, 560);
+        DE0_read(&sensors);
+        
+        SaturateValue(sensors.dp_right, 560);
+        SaturateValue(sensors.dp_left, 560);
+        
+        // maximum velocity at dPostition = 560 is vel = 703
+        velR = (float)((6135.92 * sensors.dp_right) / sensors.dt_right) ;
         
         // maximum velocity at dPostition = 560 is vel = 703
-        vel = (float)((6135.92 * dPosition) / dTime) ;
+        velL = (float)((6135.92 * sensors.dp_left) / sensors.dt_left) ;
+        
         
-        U = PiController(setpoint,dPosition);
+        U_right = PiControllerR(setpointR,sensors.dp_right);
+        U_left  = PiControllerL(setpointL,sensors.dp_left);
         
-        if (U >= 0)
+        // set speed and direction for right motor
+        if (U_right >= 0)
         {
-            motorDriver_forward(U);
+            motorDriver_R_forward(U_right);
+        }
+        else if (U_right < 0)
+        {
+            motorDriver_R_reverse(U_right);
         }
-        else if (U < 0)
+        
+        // set speed and direction of left motor
+        if (U_left >= 0)
         {
-            motorDriver_reverse(U);
+            motorDriver_L_forward(U_left);
+        }
+        else if (U_left < 0)
+        {
+            motorDriver_L_reverse(U_left);
         }
            
     }
--- a/PiControlThread.h	Mon Feb 19 17:42:33 2018 +0000
+++ b/PiControlThread.h	Fri Feb 23 20:58:34 2018 +0000
@@ -1,9 +1,9 @@
 /******************************************************************************/
 // ECE4333
-// LAB Partner 1: Ahmed Sobhy - ID: 3594449
-// LAB Partner 2: Brandon Kingman - ID: 3470444
-// Project: Autonomous Robot Design
-// Instructor: Prof. Chris Diduch
+// LAB Partner 1:   Ahmed Sobhy - ID: 3594449
+// LAB Partner 2:   Brandon Kingman - ID: 3470444
+// Project:         Autonomous Robot Design
+// Instructor:      Prof. Chris Diduch
 /******************************************************************************/
 
 #ifndef PERIODIC_INT_H
--- a/WatchdogThread.cpp	Mon Feb 19 17:42:33 2018 +0000
+++ b/WatchdogThread.cpp	Fri Feb 23 20:58:34 2018 +0000
@@ -1,9 +1,9 @@
 /******************************************************************************/
 // ECE4333
-// LAB Partner 1: Ahmed Sobhy - ID: 3594449
-// LAB Partner 2: Brandon Kingman - ID: 3470444
-// Project: Autonomous Robot Design
-// Instructor: Prof. Chris Diduch
+// LAB Partner 1:   Ahmed Sobhy - ID: 3594449
+// LAB Partner 2:   Brandon Kingman - ID: 3470444
+// Project:         Autonomous Robot Design
+// Instructor:      Prof. Chris Diduch
 /******************************************************************************/
 // filename: WatchdogThread.cpp
 // file content description: 
--- a/WatchdogThread.h	Mon Feb 19 17:42:33 2018 +0000
+++ b/WatchdogThread.h	Fri Feb 23 20:58:34 2018 +0000
@@ -1,9 +1,9 @@
 /******************************************************************************/
 // ECE4333
-// LAB Partner 1: Ahmed Sobhy - ID: 3594449
-// LAB Partner 2: Brandon Kingman - ID: 3470444
-// Project: Autonomous Robot Design
-// Instructor: Prof. Chris Diduch
+// LAB Partner 1:   Ahmed Sobhy - ID: 3594449
+// LAB Partner 2:   Brandon Kingman - ID: 3470444
+// Project:         Autonomous Robot Design
+// Instructor:      Prof. Chris Diduch
 /******************************************************************************/
 
 #ifndef WDTHREAD_H_
--- a/main.cpp	Mon Feb 19 17:42:33 2018 +0000
+++ b/main.cpp	Fri Feb 23 20:58:34 2018 +0000
@@ -1,9 +1,9 @@
 /******************************************************************************/
 // ECE4333
-// LAB Partner 1: Ahmed Sobhy - ID: 3594449
-// LAB Partner 2: Brandon Kingman - ID: 3470444
-// Project: Autonomous Robot Design
-// Instructor: Prof. Chris Diduch
+// LAB Partner 1:   Ahmed Sobhy - ID: 3594449
+// LAB Partner 2:   Brandon Kingman - ID: 3470444
+// Project:         Autonomous Robot Design
+// Instructor:      Prof. Chris Diduch
 /******************************************************************************/
 
 #include "mbed.h"
--- a/ui.cpp	Mon Feb 19 17:42:33 2018 +0000
+++ b/ui.cpp	Fri Feb 23 20:58:34 2018 +0000
@@ -1,9 +1,9 @@
 /******************************************************************************/
 // ECE4333
-// LAB Partner 1: Ahmed Sobhy - ID: 3594449
-// LAB Partner 2: Brandon Kingman - ID: 3470444
-// Project: Autonomous Robot Design
-// Instructor: Prof. Chris Diduch
+// LAB Partner 1:   Ahmed Sobhy - ID: 3594449
+// LAB Partner 2:   Brandon Kingman - ID: 3470444
+// Project:         Autonomous Robot Design
+// Instructor:      Prof. Chris Diduch
 /******************************************************************************/
 // filename: ui.cpp
 // file content description:
@@ -17,14 +17,17 @@
 Serial bluetooth(p9,p10); // Define the bluetooth channel and IO pins
 Serial pc(USBTX, USBRX); // Pins (tx, rx) for PC serial channel
 
-Mutex setpoint_mutex;
+Mutex setpointR_mutex;
+Mutex setpointL_mutex;
 
 // speed
-int setpoint = 0;
-float total_time_passed = 0;
+int setpointR = 0;
+int setpointL = 0;
+
 // variable to store character recieved from terminal
 char x;
 
+/*
 extern int16_t dPosition, dTime;
 extern float Ki; 
 extern float Kp;
@@ -33,6 +36,7 @@
 extern int32_t xState;
 extern int32_t u;
 extern int time_passed;
+*/
 
 int16_t position;
 
@@ -65,8 +69,10 @@
     bluetooth.printf("\r\n**** DC Motor Control using PWM ****");
     bluetooth.printf("\r\n************************************");
     bluetooth.printf("\r\n-Enter r to reset the watchdog timer");
-    bluetooth.printf("\r\n-press w to increase motor speed");
-    bluetooth.printf("\r\n-press s to decrease motor speed");
+    bluetooth.printf("\r\n-press w to increase motor speedR");
+    bluetooth.printf("\r\n-press s to decrease motor speedR");
+    bluetooth.printf("\r\n-press i to increase motor speedL");
+    bluetooth.printf("\r\n-press k to decrease motor speedL");
 }
 
 
@@ -142,57 +148,68 @@
         if(x == 'r') {
             // reset watchdog timer
             WatchdogReset();
-            setpoint = 0;
+            setpointR = 0;
+            setpointL = 0;
             bluetooth.printf("\r\nWatchdog has been reset");
         }
-
+        
+/******************************RIGHT MOTOR*************************************/
         // if w is pressed increase the speed
         // by incrementing u
         else if(x == 'w') {
-            setpoint_mutex.lock();
-            if ( setpoint < 560 ) 
+            setpointR_mutex.lock();
+            if ( setpointR < 560 ) 
             {
                 //setpoint = setpoint + SPEED_STEP;
-                setpoint = 280;
+                setpointR = 280;
             }
-            setpoint_mutex.unlock();
-
-            // display speed
-            //bluetooth.printf("\r\n %5d", setpoint);
+            setpointR_mutex.unlock();
         }
 
         // if s is pressed decrease the speed
         // by decrementing u
         else if(x == 's') {
 
-            setpoint_mutex.lock();
-            if (setpoint > -560) 
+            setpointR_mutex.lock();
+            if (setpointR > -560) 
             {
-                setpoint = -280;
+                setpointR = -280;
                 //setpoint = setpoint - SPEED_STEP;
             }
                 
-            setpoint_mutex.unlock();
+            setpointR_mutex.unlock();
 
             // display speed
-            bluetooth.printf("\r\n %5d", setpoint);
+            bluetooth.printf("\r\n %5d", setpointR);
         }
+
+/******************************LEFT MOTOR**************************************/        
         else if (x=='i')
         {
-            Ki = Ki + 0.01;
+            setpointL_mutex.lock();
+            if ( setpointL < 560 ) 
+            {
+                //setpoint = setpoint + SPEED_STEP;
+                setpointL = 280;
+            }
+            setpointL_mutex.unlock();
+            
         }
         else if (x=='k')
         {
-            Ki = Ki - 0.01;
+            setpointL_mutex.lock();
+            if (setpointL > -560) 
+            {
+                setpointL = -280;
+                //setpoint = setpoint - SPEED_STEP;
+            }
+                
+            setpointL_mutex.unlock();
+
+            // display speed
+            bluetooth.printf("\r\n %5d", setpointL);
         }
-        else if (x=='o')
-        {
-            Kp = Kp + 0.1;
-        }
-        else if (x=='l')
-        {
-            Kp = Kp - 0.1;    
-        }
+/******************************END MOTOR SETPOINT******************************/
         
         // error wrong input
         else {
@@ -200,20 +217,18 @@
         }
     }   
     
-    position += dPosition;
-    
     
     
     //bluetooth.printf("\r\nPos: %d, dP: %d, dT: %d, Kp: %f, Ki: %f, vel: %d, e: %d", position, dPosition, dTime, Kp, Ki, vel, e);
     
     //bluetooth.printf("\r\ndP: %d, vel: %d, (Kp*e) Kp: %f, (Ki*xState) Ki: %f, e: %d, xState: %d, u: %d", dPosition, vel, Kp, Ki, e, xState , u);
     
-    
+    /*
     bluetooth.printf("\r\n %d,   ", e);
     bluetooth.printf("%d,   ", dPosition);
     bluetooth.printf("%d,   ", xState);
     bluetooth.printf("%d,   ", u);
-    
+    */
     
     //bluetooth.printf("\r\ne: %d, Pos: %d, dP: %d, xState: %d, u: %d, dT: %d", e, position, dPosition, xState, u, dTime);
     
--- a/ui.h	Mon Feb 19 17:42:33 2018 +0000
+++ b/ui.h	Fri Feb 23 20:58:34 2018 +0000
@@ -1,9 +1,9 @@
 /******************************************************************************/
 // ECE4333
-// LAB Partner 1: Ahmed Sobhy - ID: 3594449
-// LAB Partner 2: Brandon Kingman - ID: 3470444
-// Project: Autonomous Robot Design
-// Instructor: Prof. Chris Diduch
+// LAB Partner 1:   Ahmed Sobhy - ID: 3594449
+// LAB Partner 2:   Brandon Kingman - ID: 3470444
+// Project:         Autonomous Robot Design
+// Instructor:      Prof. Chris Diduch
 /******************************************************************************/
 
 #ifndef UI_H