ManualControl
Dependencies: TPixy-Interface
Fork of MbedOS_Robot_Team by
Revision 9:fe56b888985c, committed 2018-02-23
- 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
--- 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