The subsystem design/basis for the final project
Dependencies: mbed-rtos mbed-src pixylib
Revision 19:05b8123905fb, committed 2017-04-24
- Comitter:
- balsamfir
- Date:
- Mon Apr 24 21:37:50 2017 +0000
- Parent:
- 18:501f1007a572
- Commit message:
- Commit before share
Changed in this revision
diff -r 501f1007a572 -r 05b8123905fb PeriodicPI.h --- a/PeriodicPI.h Thu Apr 07 13:19:29 2016 +0000 +++ b/PeriodicPI.h Mon Apr 24 21:37:50 2017 +0000 @@ -3,8 +3,6 @@ #include "mbed.h" - - class PeriodicPI { public:
diff -r 501f1007a572 -r 05b8123905fb global.cpp --- a/global.cpp Thu Apr 07 13:19:29 2016 +0000 +++ b/global.cpp Mon Apr 24 21:37:50 2017 +0000 @@ -5,16 +5,16 @@ DigitalOut led2(LED2); DigitalOut led3(LED3); DigitalOut led4(LED4); -DigitalOut leftDir(p24); // +DigitalOut leftDir(p23); // DigitalOut rightDir(p22); // -DigitalOut spiReset(p8); //changed -DigitalOut ioReset(p14); // +DigitalOut spiReset(p14); //changed +DigitalOut ioReset(p15); // // Comunication SPI deSpi(p5, p6, p7); Pixy pixy(Pixy::SPI, p11, p12, p13); -//Serial pc(USBTX, USBRX); // PC serial channel -Serial pc(p28, p27); // Bluetooth serial channel +Serial pc(USBTX, USBRX); // PC serial channel +//Serial pc(p9, p10); // Bluetooth serial channel // Control PeriodicPI leftMotorPI(MOTOR_PERIOD, MOTOR_KP, MOTOR_KI); @@ -23,18 +23,15 @@ PeriodicPI xPI(NAVIGATION_PERIOD, STEERING_KP, STEERING_KI); // Other -PwmOut leftPwm(p23); // -PwmOut rightPwm(p21); // -InterruptIn bumper(p25); +PwmOut leftPwm(p24); +PwmOut rightPwm(p21); -// Converts measurements from the QE2 to rads/sec -float QE2RadsPerSec(short counts, short time) { - return ((float)counts*122.62)/time; +// Converts measurements from the FPGA to rads/sec +float RadsPerSec(short counts, short time) { + return ((float)counts*26.558)/time; } // Returns the last character -char flushBuffer(void) { - char ch; - while (pc.readable()) pc.putc(pc.getc()); - return ch; +void FlushBuffer(void) { + while (pc.readable()) pc.getc(); } \ No newline at end of file
diff -r 501f1007a572 -r 05b8123905fb global.h --- a/global.h Thu Apr 07 13:19:29 2016 +0000 +++ b/global.h Mon Apr 24 21:37:50 2017 +0000 @@ -10,27 +10,29 @@ // Preprocessor Definitions // ---------------------------------------------------------------- -#define SPEED_MAX 40 // rads/s +#define SPEED_MAX 2 // rads/s #define MAX_BLOCKS 1 #define X_SETPOINT 160 -#define HEIGHT_SETPOINT 90 -#define TARGET_DECIMAL 10 +#define HEIGHT_SETPOINT 120 +#define TARGET_DECIMAL 1 + +// #define DEBUG 1 #define PWM_PERIOD 0.001 -#define MOTOR_PERIOD 0.001 +#define MOTOR_PERIOD 0.01 #define NAVIGATION_PERIOD 0.0167 // 60 times/sec -#define MOTOR_KP 0.000120 -#define MOTOR_KI 0.0000001 +#define MOTOR_KP 0.0001 // 0.0001 +#define MOTOR_KI 0.0005 // 0.0001 //0.00001 //0.0000001 #define STEERING_KP 0.10 #define STEERING_KI 0 -#define SPEED_KP 0.6 +#define SPEED_KP 0.1 #define SPEED_KI 0 #define MOTOR_SAMPLES 300 -#define NAVIGATION_SAMPLES 240 +#define NAVIGATION_SAMPLES 200 // Global variables @@ -59,8 +61,7 @@ // Other extern PwmOut leftPwm; extern PwmOut rightPwm; -extern InterruptIn bumper; // External interrupt pin declared as Bumper // Method prototypes -char flushBuffer(void); -float QE2RadsPerSec(short counts, short time); \ No newline at end of file +void FlushBuffer(void); +float RadsPerSec(short counts, short time); \ No newline at end of file
diff -r 501f1007a572 -r 05b8123905fb main.cpp --- a/main.cpp Thu Apr 07 13:19:29 2016 +0000 +++ b/main.cpp Mon Apr 24 21:37:50 2017 +0000 @@ -4,25 +4,7 @@ #include "global.h" #include "robot.h" -// Definitions -// ---------------------------------------------------------------- -enum Mode { - AUTO_TRACK = '0', - MANUAL_CONTROL = '1', - SYSTEM_TUNING = '2' -}; - -void PrintMenu(Serial *pc); - - -// Wiring - TODO -// ---------------------------------------------------------------- -// -// -// -// -// -// +void PrintMenu(void); // Main Program // ---------------------------------------------------------------- @@ -31,14 +13,14 @@ InitRobot(); while (1) { - PrintMenu(&pc); + PrintMenu(); mode = pc.getc(); pc.printf("\r\n\r\n"); - if ((mode == '0')||(mode == 'a')) { + if (mode == 'a') { AutoTrack(); - } else if ((mode == '1')||(mode == 'm')) { + } else if (mode == 'm') { ManualControl(); - } else if (mode == '2') { + } else if (mode == 't') { Tunning(); } else { pc.printf("Error: Invalid Selection \r\n\r\n"); @@ -49,15 +31,15 @@ // Other Functions // ---------------------------------------------------------------- -void PrintMenu(Serial *pc){ - pc->printf("\e[1;1H\e[2J"); - pc->printf("Select Mode: \r\n\r\n"); +void PrintMenu(){ + pc.printf("\e[1;1H\e[2J"); + pc.printf("Select Mode: \r\n\r\n"); - pc->printf("---------------------------------------------------------------- \r\n"); - pc->printf("0. Automated Tracking \r\n"); - pc->printf("1. Manual Control \r\n"); - pc->printf("2. System Tuning \r\n"); - pc->printf("---------------------------------------------------------------- \r\n\r\n"); + pc.printf("---------------------------------------------------------------- \r\n"); + pc.printf("a. Automated Tracking \r\n"); + pc.printf("m. Manual Control \r\n"); + pc.printf("t. System Tuning \r\n"); + pc.printf("---------------------------------------------------------------- \r\n\r\n"); - pc->printf("=> "); + pc.printf("=> "); }
diff -r 501f1007a572 -r 05b8123905fb robot.cpp --- a/robot.cpp Thu Apr 07 13:19:29 2016 +0000 +++ b/robot.cpp Mon Apr 24 21:37:50 2017 +0000 @@ -59,8 +59,6 @@ rightPwm.pulsewidth(0); motorMutex = osMutexCreate(osMutex(motorMutex)); - - bumper.rise(&CollisionISR); // Attach interrupt handler to rising edge of Bumper // Start execution of the threads MotorThread, and NavigationThread: navigationId = osThreadCreate(osThread(NavigationThread), NULL); @@ -73,15 +71,17 @@ //SPI Initialization pc.printf("\n\rStarting SPI..."); deSpi.format(16,1); // 16 bit mode 1 + deSpi.frequency(500000); ioReset = 0; ioReset = 1; wait_us(10); ioReset = 0; spiReset = 0; spiReset = 1; wait_us(10); spiReset = 0; pc.printf("\n\rDevice Id: %d \r\n", deSpi.write(0x8004)); // Read count & time for both motors + FlushBuffer(); } +// Setup robot for auto tracking and listen for commands void AutoTrack(void) { char key; - bool isReady; motorPeriodicInt.attach(&MotorISR, MOTOR_PERIOD); sensorPeriodicInt.attach(&NavigationISR, NAVIGATION_PERIOD); @@ -92,26 +92,21 @@ if (key == 'q') { ShutDown(); return; - } else if ((key == '1')||(key == 'm')) { + } else if (key == 'm') { ShutDown(); ManualControl(); - } else if (key == '~') { - isReady = true; - } + } } - if (isReady) { - pc.printf("X: %d, Height: %d \r\n", x, height); - pc.printf("Speed: %f \r\nSteering: %f \r\n", speed, steering); - isReady = false; - pc.putc('~'); - } + pc.printf("X: %d, Height: %d \r\n", x, height); + pc.printf("Speed: %f \r\nSteering: %f \r\n", speed, steering); + Thread::wait(250); } } +// Setup robot for manual control and listen for commands void ManualControl(void) { - bool isReady; char key; motorPeriodicInt.attach(&MotorISR, MOTOR_PERIOD); float speedRate, steeringRate; @@ -123,25 +118,25 @@ if (key == 'q') { ShutDown(); return; - } else if ((key == '0')||(key == 'a')) { + } else if (key == 'a') { ShutDown(); AutoTrack(); - } else if (key == '~') { - isReady = true; + } else if (key == '8') { + speedRate = (speedRate < 1)? speedRate + 0.1 : 1; + } else if (key == '5') { + speedRate = (speedRate > -1)? speedRate - 0.1 : -1; + } else if (key == '6') { + steeringRate = (steeringRate < 1)? steeringRate + 0.1 : 1; + } else if (key == '4') { + steeringRate = (steeringRate > -1)? steeringRate - 0.1 : -1; } + + pc.printf("%.2f,%.2f\r\n", speedRate, steeringRate); } - if (isReady) { - pc.scanf("%f :: %f", &speedRate, &steeringRate); - speed = SPEED_MAX * speedRate; - steering = (SPEED_MAX * steeringRate); - isReady = false; - pc.putc('~'); - } - - osMutexWait(motorMutex, osWaitForever); - leftMotor = speed - steering; - rightMotor = speed + steering; + osMutexWait(motorMutex, osWaitForever); + leftMotor = SPEED_MAX*(speedRate + steeringRate); + rightMotor = SPEED_MAX*(speedRate - steeringRate); osMutexRelease(motorMutex); } } @@ -264,13 +259,13 @@ leftMotorPI.Reset(); rightMotorPI.Reset(); pc.printf("Robot Shutdown... \r\n\r\n"); + FlushBuffer(); } // Threads // ---------------------------------------------------------------- void NavigationThread(void const *argument) { int count; - int missingCount; while (1) { osSignalWait(navigationSignal, osWaitForever); // Go to sleep until navigation signal is set high by ISR @@ -279,8 +274,7 @@ // If target returned if (count && (pixy.blocks[0].signature == TARGET_DECIMAL)) { - missingCount = 0; - height = pixy.blocks[0].height; //use this for now + height = pixy.blocks[0].height; x = pixy.blocks[0].x; speed = heightPI.Run(HEIGHT_SETPOINT-height, SPEED_MAX); @@ -288,25 +282,10 @@ // Give setpoints to MotorThread osMutexWait(motorMutex, osWaitForever); - if (speed >= 0) { - leftMotor = speed - steering; - rightMotor = speed + steering; - } else { - leftMotor = speed - steering; - rightMotor = speed + steering; - } + leftMotor = speed - steering; + rightMotor = speed + steering; osMutexRelease(motorMutex); - } else { - // When the robot can't see the target spin in last known direction - if (missingCount >= 30) { - osMutexWait(motorMutex, osWaitForever); - leftMotor = (leftMotor > rightMotor)? SPEED_MAX/3 : -SPEED_MAX/3; - rightMotor = (rightMotor > leftMotor)? SPEED_MAX/3 : -SPEED_MAX/3; - osMutexRelease(motorMutex); - } else { - missingCount++; - } - } + } if (isTunning && (navigationSample < NAVIGATION_SAMPLES)) { speedResponse[navigationSample] = speed; @@ -319,11 +298,11 @@ void MotorThread(void const *argument) { float leftSet, rightSet, angVel; float timeOnLeft, timeOnRight; + short loop; short dP, dt; while (1) { osSignalWait(motorSignal, osWaitForever); // Go to sleep until motor signal is set high by ISR - led2= !led2; // Alive status - led2 toggles each time MotorControlThread is signaled. // Get setpoints from navigation osMutexWait(motorMutex, osWaitForever); @@ -331,21 +310,22 @@ rightSet = rightMotor; osMutexRelease(motorMutex); + // Run PI control on right motor + dP = deSpi.write(0); + dt = deSpi.write(0); + angVel = RadsPerSec(dP, dt); // motors have opposite orientations + timeOnRight = rightMotorPI.Run(rightSet-angVel, PWM_PERIOD/2); + // Run PI control on left motor dP = deSpi.write(0); dt = deSpi.write(0); - angVel = QE2RadsPerSec(dP, dt); - timeOnLeft = leftMotorPI.Run(leftSet-angVel, PWM_PERIOD); + angVel = -RadsPerSec(dP, dt); + timeOnLeft = leftMotorPI.Run(leftSet-angVel, PWM_PERIOD/2); - // Run PI control on right motor - dP = deSpi.write(0); - dt = deSpi.write(0); - angVel = -QE2RadsPerSec(dP, dt); // motors have opposite orientations - timeOnRight = rightMotorPI.Run(rightSet-angVel, PWM_PERIOD); // // Output new PWM and direction - if (timeOnLeft >= 0) leftDir = 1; - else leftDir = 0; + if (timeOnLeft >= 0) leftDir = 0; + else leftDir = 1; if (timeOnRight >= 0) rightDir = 0; else rightDir = 1; @@ -358,6 +338,10 @@ rightMotorResponse[motorSample] = timeOnRight; motorSample++; } + + #ifdef DEBUG + loop = (loop < 1/MOTOR_PERIOD)? loop + 1 : 0; + #endif } } @@ -365,9 +349,9 @@ // Interrupt Service Routines // ---------------------------------------------------------------- -// TODO: Shutdown system void WatchdogISR(void const *n) { led3=1; // Activated when the watchdog timer times out + ShutDown(); } void MotorISR(void) { @@ -376,10 +360,4 @@ void NavigationISR(void) { osSignalSet(navigationId,0x1); // Activate the signal, SensorControl, with each periodic timer interrupt -} - -// TODO: Shutdown system -void CollisionISR(void) { - led4 = 1; // Activated on collision - } \ No newline at end of file