Ian Colwell / Mbed 2 deprecated RoboticsProject

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   mbed-rtos mbed

Fork of Project by Thomas Elliott

main.cpp@12:3058f9fb09eb, 2013-03-17 (annotated)

Committer:
IanTheMBEDMaster
Date:
Sun Mar 17 18:44:47 2013 +0000
Revision:
12:3058f9fb09eb
Parent:
11:521c3e8e6491
Child:
13:aaac0105a486
Cleaned up some code, the IR input can now be seen in debug mode. An emergency stop condition has now been added as well.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
IanTheMBEDMaster 2:1c5cc180812d 1 // Robot Control Code
IanTheMBEDMaster 2:1c5cc180812d 2 // Tom Elliott and Ian Colwell
IanTheMBEDMaster 2:1c5cc180812d 3
IanTheMBEDMaster 2:1c5cc180812d 4
LtBarbershop 1:3a40c918ff41 5 #include "mbed.h"
LtBarbershop 1:3a40c918ff41 6 #include "rtos.h"
LtBarbershop 1:3a40c918ff41 7
IanTheMBEDMaster 2:1c5cc180812d 8 // --- Constants
IanTheMBEDMaster 2:1c5cc180812d 9 #define Dummy 0
IanTheMBEDMaster 6:5200ce9fa5a7 10
IanTheMBEDMaster 7:751d5e3e9cab 11 //#define PWMPeriod 0.0005 // orignally 0.001 (gave a high pitched whine)
IanTheMBEDMaster 7:751d5e3e9cab 12 const float PWMPeriod = 0.00005;
LtBarbershop 5:7108ac9e8182 13 // Control Update = 50ms (time should be 609/610) (if we want to change this we also have to change the way feedback speed is calculated)
IanTheMBEDMaster 6:5200ce9fa5a7 14 //#define ControlUpdate 0.05
IanTheMBEDMaster 6:5200ce9fa5a7 15 const float ControlUpdate = 0.05;
LtBarbershop 4:03bf5bdca9fb 16 #define EncoderTime 610
LtBarbershop 5:7108ac9e8182 17
LtBarbershop 1:3a40c918ff41 18
IanTheMBEDMaster 2:1c5cc180812d 19 // --- Function prototypes
LtBarbershop 1:3a40c918ff41 20 void PiControllerISR(void);
LtBarbershop 1:3a40c918ff41 21 void WdtFaultISR(void);
LtBarbershop 1:3a40c918ff41 22 void ExtCollisionISR(void);
LtBarbershop 1:3a40c918ff41 23 void PiControlThread(void const *argument);
LtBarbershop 1:3a40c918ff41 24 void ExtCollisionThread(void const *argument);
LtBarbershop 1:3a40c918ff41 25 void Watchdog(void const *n);
IanTheMBEDMaster 2:1c5cc180812d 26 void InitializeSystem();
IanTheMBEDMaster 2:1c5cc180812d 27 void InitializeEncoder();
IanTheMBEDMaster 2:1c5cc180812d 28 void InitializePWM();
IanTheMBEDMaster 2:1c5cc180812d 29 void PwmSetOut(float d, float T);
IanTheMBEDMaster 7:751d5e3e9cab 30 void GetSpeeds();
IanTheMBEDMaster 2:1c5cc180812d 31 void SetLeftMotorSpeed(float u);
IanTheMBEDMaster 2:1c5cc180812d 32 void SetRightMotorSpeed(float u);
LtBarbershop 11:521c3e8e6491 33 void DisplayMenu();
LtBarbershop 11:521c3e8e6491 34 void Ramp(float speed, unsigned int time, unsigned short motor);
LtBarbershop 11:521c3e8e6491 35 void IRChecker();
LtBarbershop 10:a3ecedc8d9d7 36 void BTInit();
LtBarbershop 1:3a40c918ff41 37
IanTheMBEDMaster 6:5200ce9fa5a7 38 // Global variables
LtBarbershop 5:7108ac9e8182 39 float u1 = 0;
LtBarbershop 5:7108ac9e8182 40 float u2 = 0;
IanTheMBEDMaster 7:751d5e3e9cab 41 float cSetL = 0;
IanTheMBEDMaster 7:751d5e3e9cab 42 float cSetR = 0;
LtBarbershop 5:7108ac9e8182 43 float userSetL = 0;
LtBarbershop 5:7108ac9e8182 44 float userSetR = 0;
LtBarbershop 4:03bf5bdca9fb 45 int startup = 0;
LtBarbershop 4:03bf5bdca9fb 46 float aeL = 0;
LtBarbershop 4:03bf5bdca9fb 47 float aeR = 0;
LtBarbershop 8:d65cba3bfc0e 48 float RecV[100]; // Record Feedback Speed
LtBarbershop 8:d65cba3bfc0e 49 float RecU[100]; // Record Motor Input
LtBarbershop 8:d65cba3bfc0e 50 float RecX[100]; // Record Integrator Output
LtBarbershop 8:d65cba3bfc0e 51 float RecE[100]; // Record Error
LtBarbershop 8:d65cba3bfc0e 52 int RecCount = 0; // Record Counter
LtBarbershop 11:521c3e8e6491 53 unsigned short action;
IanTheMBEDMaster 6:5200ce9fa5a7 54 Mutex Var_Lock;
IanTheMBEDMaster 6:5200ce9fa5a7 55
IanTheMBEDMaster 6:5200ce9fa5a7 56 // global variables to eventually be removed
IanTheMBEDMaster 7:751d5e3e9cab 57 short dPositionLeft, dTimeLeft, dPositionRight, dTimeRight;
IanTheMBEDMaster 6:5200ce9fa5a7 58 float fbSpeedL, fbSpeedR;
LtBarbershop 5:7108ac9e8182 59 float eL = 0;
LtBarbershop 5:7108ac9e8182 60 float eR = 0;
IanTheMBEDMaster 2:1c5cc180812d 61
IanTheMBEDMaster 2:1c5cc180812d 62 // --- Processes and threads
LtBarbershop 1:3a40c918ff41 63 int32_t SignalPi, SignalWdt, SignalExtCollision;
LtBarbershop 1:3a40c918ff41 64 osThreadId PiControl,WdtFault,ExtCollision;
LtBarbershop 1:3a40c918ff41 65 osThreadDef(PiControlThread, osPriorityNormal, DEFAULT_STACK_SIZE);
LtBarbershop 1:3a40c918ff41 66 osThreadDef(ExtCollisionThread, osPriorityNormal, DEFAULT_STACK_SIZE);
LtBarbershop 1:3a40c918ff41 67 osTimerDef(Wdtimer, Watchdog);
LtBarbershop 1:3a40c918ff41 68
IanTheMBEDMaster 2:1c5cc180812d 69 /* PIN-OUT
IanTheMBEDMaster 2:1c5cc180812d 70
IanTheMBEDMaster 2:1c5cc180812d 71 MOSI Quad Enc 5|-|
IanTheMBEDMaster 2:1c5cc180812d 72 MISO Quad Enc 6|-|
IanTheMBEDMaster 2:1c5cc180812d 73 SCK Quad Enc 7|-|
IanTheMBEDMaster 2:1c5cc180812d 74 SPI Start Quad E 8|-|
IanTheMBEDMaster 2:1c5cc180812d 75 SPI Reset Quad E 9|-|
IanTheMBEDMaster 12:3058f9fb09eb 76 Emergency Stop 10|-|
IanTheMBEDMaster 2:1c5cc180812d 77 Bluetooth tx 13|-|28
IanTheMBEDMaster 2:1c5cc180812d 78 Bluetooth rx 14|-|27
LtBarbershop 4:03bf5bdca9fb 79 15|-|26 Brake, Left Motor, M1
LtBarbershop 4:03bf5bdca9fb 80 16|-|25 Dir, Left Motor, M1
LtBarbershop 4:03bf5bdca9fb 81 17|-|24 PWM, Left Motor, M1
LtBarbershop 4:03bf5bdca9fb 82 18|-|23 Brake, Right Motor, M2
LtBarbershop 9:0eb7b173d6c3 83 Front IR 19|-|22 Dir, Right Motor, M2
LtBarbershop 9:0eb7b173d6c3 84 Rear IR 20|-|21 PWM, Right Motor, M2
IanTheMBEDMaster 2:1c5cc180812d 85 */
IanTheMBEDMaster 2:1c5cc180812d 86
IanTheMBEDMaster 2:1c5cc180812d 87 // --- IO Port Configuration
LtBarbershop 1:3a40c918ff41 88 DigitalOut led1(LED1);
LtBarbershop 1:3a40c918ff41 89 DigitalOut led2(LED2);
LtBarbershop 1:3a40c918ff41 90 DigitalOut led3(LED3);
LtBarbershop 1:3a40c918ff41 91 DigitalOut led4(LED4);
LtBarbershop 8:d65cba3bfc0e 92 DigitalOut dirR(p22);
LtBarbershop 8:d65cba3bfc0e 93 DigitalOut brakeR(p23);
LtBarbershop 8:d65cba3bfc0e 94 PwmOut PwmR(p21);
LtBarbershop 8:d65cba3bfc0e 95 DigitalOut dirL(p25);
LtBarbershop 8:d65cba3bfc0e 96 DigitalOut brakeL(p26);
LtBarbershop 8:d65cba3bfc0e 97 PwmOut PwmL(p24);
LtBarbershop 10:a3ecedc8d9d7 98 Serial BtS(p13, p14); // (tx, rx) for PC serial channel
LtBarbershop 1:3a40c918ff41 99 Serial pc(USBTX, USBRX); // (tx, rx) for Parani/Promi Bluetooth serial channel
IanTheMBEDMaster 2:1c5cc180812d 100 SPI DE0(p5, p6, p7); // (mosi, miso, sclk) DE0 is the SPI channel with the DE0 FPGA
IanTheMBEDMaster 2:1c5cc180812d 101 DigitalOut SpiReset(p9); // Reset for all devices within the slave SPI peripheral in the DE0 FPGA
IanTheMBEDMaster 2:1c5cc180812d 102 DigitalOut SpiStart(p8); // Places SPI interace on the DE0 FPGA into control mode
IanTheMBEDMaster 12:3058f9fb09eb 103 InterruptIn EmerStop(p10); // External interrupt pin
LtBarbershop 9:0eb7b173d6c3 104 AnalogIn IRFront(p19); // Front IR Ranger Input
LtBarbershop 9:0eb7b173d6c3 105 AnalogIn IRRear(p20); // Rear IR Ranger Input
LtBarbershop 1:3a40c918ff41 106 Ticker PeriodicInt;
IanTheMBEDMaster 2:1c5cc180812d 107
LtBarbershop 1:3a40c918ff41 108
LtBarbershop 1:3a40c918ff41 109 // ******** Main Thread ********
IanTheMBEDMaster 2:1c5cc180812d 110 int main()
IanTheMBEDMaster 2:1c5cc180812d 111 {
LtBarbershop 11:521c3e8e6491 112 char c;
LtBarbershop 11:521c3e8e6491 113
LtBarbershop 11:521c3e8e6491 114 InitializeSystem();
LtBarbershop 11:521c3e8e6491 115 BtS.printf("\r\n --- Robot Initialization Complete --- \r\n");
LtBarbershop 11:521c3e8e6491 116 DisplayMenu();
LtBarbershop 11:521c3e8e6491 117
LtBarbershop 11:521c3e8e6491 118 //BtS.printf("\n\n\rTap w-a-s-d keys for differential speed control: ");
LtBarbershop 11:521c3e8e6491 119 while(1)
LtBarbershop 11:521c3e8e6491 120 {
LtBarbershop 11:521c3e8e6491 121 if(BtS.readable())
LtBarbershop 11:521c3e8e6491 122 {
LtBarbershop 11:521c3e8e6491 123 c = BtS.getc();
LtBarbershop 11:521c3e8e6491 124
LtBarbershop 11:521c3e8e6491 125 // quit
LtBarbershop 11:521c3e8e6491 126 if (c == 'q')
LtBarbershop 11:521c3e8e6491 127 {
LtBarbershop 11:521c3e8e6491 128 action = 0;
IanTheMBEDMaster 12:3058f9fb09eb 129 Ramp(0, 1000, 0);
LtBarbershop 11:521c3e8e6491 130 DisplayMenu();
LtBarbershop 11:521c3e8e6491 131 continue;
LtBarbershop 11:521c3e8e6491 132 }
LtBarbershop 11:521c3e8e6491 133
LtBarbershop 11:521c3e8e6491 134 if (action == 0)
LtBarbershop 11:521c3e8e6491 135 {
LtBarbershop 11:521c3e8e6491 136 // choose a menu selection
LtBarbershop 11:521c3e8e6491 137 switch(c)
LtBarbershop 11:521c3e8e6491 138 {
LtBarbershop 11:521c3e8e6491 139 case 'd':
LtBarbershop 11:521c3e8e6491 140 action = 1;
IanTheMBEDMaster 12:3058f9fb09eb 141 BtS.printf("\n\rTap w-a-s-d keys for differential speed control\r\nPress 'q' to quit \r\n");
LtBarbershop 11:521c3e8e6491 142 break;
LtBarbershop 11:521c3e8e6491 143 case 'w':
LtBarbershop 11:521c3e8e6491 144 action = 2;
LtBarbershop 11:521c3e8e6491 145 break;
LtBarbershop 11:521c3e8e6491 146 case '0':
LtBarbershop 11:521c3e8e6491 147 action = 3;
LtBarbershop 11:521c3e8e6491 148 break;
LtBarbershop 11:521c3e8e6491 149 case 'q':
LtBarbershop 11:521c3e8e6491 150 action = 0;
LtBarbershop 11:521c3e8e6491 151 break;
LtBarbershop 11:521c3e8e6491 152 default:
IanTheMBEDMaster 12:3058f9fb09eb 153 BtS.printf("\n\r Command not recognized \n\r");
LtBarbershop 11:521c3e8e6491 154 action = 0;
LtBarbershop 11:521c3e8e6491 155 break;
LtBarbershop 11:521c3e8e6491 156 }
LtBarbershop 11:521c3e8e6491 157 continue;
LtBarbershop 11:521c3e8e6491 158 }
LtBarbershop 11:521c3e8e6491 159
LtBarbershop 11:521c3e8e6491 160 if (action == 1)
LtBarbershop 11:521c3e8e6491 161 {
LtBarbershop 11:521c3e8e6491 162 // keyboard input to drive robot using wasd
LtBarbershop 11:521c3e8e6491 163 switch(c)
LtBarbershop 11:521c3e8e6491 164 {
LtBarbershop 11:521c3e8e6491 165 case('w'):
LtBarbershop 11:521c3e8e6491 166 userSetL = userSetL + 0.05;
LtBarbershop 11:521c3e8e6491 167 userSetR = userSetR + 0.05;
LtBarbershop 11:521c3e8e6491 168 break;
LtBarbershop 11:521c3e8e6491 169 case('s'):
LtBarbershop 11:521c3e8e6491 170 userSetL = userSetL - 0.05;
LtBarbershop 11:521c3e8e6491 171 userSetR = userSetR - 0.05;
LtBarbershop 11:521c3e8e6491 172 break;
LtBarbershop 11:521c3e8e6491 173 case('a'):
LtBarbershop 11:521c3e8e6491 174 userSetL = userSetL - 0.025;
LtBarbershop 11:521c3e8e6491 175 userSetR = userSetR + 0.025;
LtBarbershop 11:521c3e8e6491 176 break;
LtBarbershop 11:521c3e8e6491 177 case('d'):
LtBarbershop 11:521c3e8e6491 178 userSetL = userSetL + 0.025;
LtBarbershop 11:521c3e8e6491 179 userSetR = userSetR - 0.025;
LtBarbershop 11:521c3e8e6491 180 break;
LtBarbershop 11:521c3e8e6491 181 }
IanTheMBEDMaster 12:3058f9fb09eb 182 continue;
LtBarbershop 11:521c3e8e6491 183 }
LtBarbershop 11:521c3e8e6491 184
LtBarbershop 11:521c3e8e6491 185 if (action == 2)
LtBarbershop 11:521c3e8e6491 186 {
LtBarbershop 11:521c3e8e6491 187 // keyboard input to wall following
LtBarbershop 11:521c3e8e6491 188 }
LtBarbershop 11:521c3e8e6491 189 if (action == 3)
LtBarbershop 11:521c3e8e6491 190 {
LtBarbershop 11:521c3e8e6491 191 // keyboard input to debug mode
LtBarbershop 11:521c3e8e6491 192 float newSpeed;
LtBarbershop 11:521c3e8e6491 193 BtS.printf("\n\r Enter a motor speed (with sign as direction:\n\r");
LtBarbershop 11:521c3e8e6491 194 BtS.scanf("%f", &newSpeed);
LtBarbershop 11:521c3e8e6491 195
LtBarbershop 11:521c3e8e6491 196 BtS.printf("%f", newSpeed);
LtBarbershop 11:521c3e8e6491 197 Ramp(newSpeed, 1000, 0);
LtBarbershop 11:521c3e8e6491 198 //userSetR = userSetL;
IanTheMBEDMaster 12:3058f9fb09eb 199 }
LtBarbershop 11:521c3e8e6491 200 }// close if(BtS.readable())
LtBarbershop 11:521c3e8e6491 201
LtBarbershop 11:521c3e8e6491 202
LtBarbershop 11:521c3e8e6491 203 if (action == 2)
LtBarbershop 11:521c3e8e6491 204 {
LtBarbershop 11:521c3e8e6491 205 IRChecker();
LtBarbershop 11:521c3e8e6491 206 action = 0;
LtBarbershop 11:521c3e8e6491 207 }
LtBarbershop 11:521c3e8e6491 208 if (action == 3)
LtBarbershop 11:521c3e8e6491 209 {
LtBarbershop 11:521c3e8e6491 210 // display debug messages
LtBarbershop 11:521c3e8e6491 211
IanTheMBEDMaster 12:3058f9fb09eb 212 float IRF, IRR;
IanTheMBEDMaster 12:3058f9fb09eb 213 IRF = IRFront.read();
IanTheMBEDMaster 12:3058f9fb09eb 214 IRR = IRRear.read();
IanTheMBEDMaster 12:3058f9fb09eb 215
IanTheMBEDMaster 12:3058f9fb09eb 216 Var_Lock.lock();
LtBarbershop 11:521c3e8e6491 217 BtS.printf("Pos. L: %d R: %d \n\r", dPositionLeft, dPositionRight);
LtBarbershop 11:521c3e8e6491 218 BtS.printf("Time L: %d R: %d \n\r", dTimeLeft, dTimeRight);
LtBarbershop 11:521c3e8e6491 219 BtS.printf("fbs L: %f R: %f \n\r", fbSpeedL, fbSpeedR);
LtBarbershop 11:521c3e8e6491 220 BtS.printf("e L: %f R: %f \r\n", eL, eR);
LtBarbershop 11:521c3e8e6491 221 BtS.printf("Ae L: %f R: %f \n\r", aeL, aeR);
IanTheMBEDMaster 12:3058f9fb09eb 222 BtS.printf("cSP L: %f R: %f \n\r", cSetL, cSetR);
IanTheMBEDMaster 12:3058f9fb09eb 223 BtS.printf("IR F: %f R: %f \n\r\n", IRF, IRR);
IanTheMBEDMaster 12:3058f9fb09eb 224 Var_Lock.unlock();
LtBarbershop 11:521c3e8e6491 225
LtBarbershop 11:521c3e8e6491 226 Thread::wait(2000);
LtBarbershop 11:521c3e8e6491 227 }
LtBarbershop 11:521c3e8e6491 228
LtBarbershop 11:521c3e8e6491 229
LtBarbershop 11:521c3e8e6491 230 /*
IanTheMBEDMaster 2:1c5cc180812d 231 InitializeSystem();
LtBarbershop 10:a3ecedc8d9d7 232 BTInit();
LtBarbershop 10:a3ecedc8d9d7 233 BtS.printf("\r\n --- Robot Initialization Complete --- \r\n");
IanTheMBEDMaster 2:1c5cc180812d 234
LtBarbershop 10:a3ecedc8d9d7 235 BtS.printf("\n\n\rTap w-a-s-d keys for differential speed control: ");
LtBarbershop 10:a3ecedc8d9d7 236 char x;
IanTheMBEDMaster 2:1c5cc180812d 237 while(1)
IanTheMBEDMaster 2:1c5cc180812d 238 {
LtBarbershop 10:a3ecedc8d9d7 239 Thread::wait(20);
LtBarbershop 10:a3ecedc8d9d7 240
IanTheMBEDMaster 12:3058f9fb09eb 241
LtBarbershop 10:a3ecedc8d9d7 242
LtBarbershop 4:03bf5bdca9fb 243
IanTheMBEDMaster 12:3058f9fb09eb 244
LtBarbershop 11:521c3e8e6491 245 if(BtS.readable())
LtBarbershop 1:3a40c918ff41 246 {
LtBarbershop 11:521c3e8e6491 247 c = BtS.getc();
LtBarbershop 8:d65cba3bfc0e 248 if (c == 'r')
LtBarbershop 8:d65cba3bfc0e 249 {
LtBarbershop 8:d65cba3bfc0e 250 userSetL = 0.2;
LtBarbershop 8:d65cba3bfc0e 251 }
LtBarbershop 8:d65cba3bfc0e 252 if (c == 'p')
LtBarbershop 8:d65cba3bfc0e 253 {
LtBarbershop 8:d65cba3bfc0e 254 if (RecCount == 100)
LtBarbershop 8:d65cba3bfc0e 255 {
LtBarbershop 11:521c3e8e6491 256 BtS.printf("\n\n\rRecV RecU RecX RecE \n\r");
LtBarbershop 8:d65cba3bfc0e 257 int i = 0;
LtBarbershop 8:d65cba3bfc0e 258 for (i = 0; i < 100; i++)
LtBarbershop 8:d65cba3bfc0e 259 {
LtBarbershop 11:521c3e8e6491 260 BtS.printf("%f, %f, %f, %f \n\r", RecV[i], RecU[i], RecX[i], RecE[i]);
LtBarbershop 8:d65cba3bfc0e 261 }
LtBarbershop 8:d65cba3bfc0e 262 }
LtBarbershop 8:d65cba3bfc0e 263
LtBarbershop 8:d65cba3bfc0e 264 }
LtBarbershop 8:d65cba3bfc0e 265
LtBarbershop 8:d65cba3bfc0e 266 if (c == 'n')
LtBarbershop 8:d65cba3bfc0e 267 {
LtBarbershop 11:521c3e8e6491 268 BtS.printf("\n\r Enter a motor speed (with sign as direction:\n\r");
LtBarbershop 11:521c3e8e6491 269 BtS.scanf("%f", &userSetL);
LtBarbershop 8:d65cba3bfc0e 270
LtBarbershop 11:521c3e8e6491 271 BtS.printf("%f", userSetL);
LtBarbershop 8:d65cba3bfc0e 272 userSetR = userSetL;
LtBarbershop 8:d65cba3bfc0e 273 }
IanTheMBEDMaster 2:1c5cc180812d 274 }
IanTheMBEDMaster 2:1c5cc180812d 275
LtBarbershop 10:a3ecedc8d9d7 276 Thread::wait(2000); Wait 2 seconds */
LtBarbershop 1:3a40c918ff41 277 }
LtBarbershop 1:3a40c918ff41 278 }
LtBarbershop 1:3a40c918ff41 279
LtBarbershop 1:3a40c918ff41 280 // ******** Control Thread ********
IanTheMBEDMaster 2:1c5cc180812d 281 void PiControlThread(void const *argument)
IanTheMBEDMaster 2:1c5cc180812d 282 {
IanTheMBEDMaster 2:1c5cc180812d 283 while (1)
IanTheMBEDMaster 2:1c5cc180812d 284 {
IanTheMBEDMaster 2:1c5cc180812d 285 osSignalWait(SignalPi, osWaitForever);
IanTheMBEDMaster 2:1c5cc180812d 286 led2= !led2; // Alive status
IanTheMBEDMaster 2:1c5cc180812d 287
IanTheMBEDMaster 6:5200ce9fa5a7 288 float prevu1, prevu2;
LtBarbershop 5:7108ac9e8182 289 //float eL = 0;
LtBarbershop 5:7108ac9e8182 290 //float eR = 0;
IanTheMBEDMaster 12:3058f9fb09eb 291 const unsigned short maxError = 1;
IanTheMBEDMaster 12:3058f9fb09eb 292 const unsigned short maxAcc = 10;
LtBarbershop 8:d65cba3bfc0e 293 // Kp = 0.1, Ki = 0.5
IanTheMBEDMaster 7:751d5e3e9cab 294 const float Kp = 0.1f;
LtBarbershop 8:d65cba3bfc0e 295 const float Ki = 0.5f;
LtBarbershop 5:7108ac9e8182 296
LtBarbershop 5:7108ac9e8182 297 prevu1 = u1;
LtBarbershop 5:7108ac9e8182 298 prevu2 = u2;
LtBarbershop 4:03bf5bdca9fb 299
IanTheMBEDMaster 6:5200ce9fa5a7 300 // read encoder and calculate speed of both motors
IanTheMBEDMaster 7:751d5e3e9cab 301 GetSpeeds();
LtBarbershop 4:03bf5bdca9fb 302
LtBarbershop 4:03bf5bdca9fb 303 // calculate error
LtBarbershop 5:7108ac9e8182 304 eL = userSetL - fbSpeedL;
LtBarbershop 5:7108ac9e8182 305 eR = userSetR - fbSpeedR;
IanTheMBEDMaster 6:5200ce9fa5a7 306
IanTheMBEDMaster 12:3058f9fb09eb 307 // prevent overflow / bound the error
LtBarbershop 5:7108ac9e8182 308 if (eL > maxError)
LtBarbershop 5:7108ac9e8182 309 {
LtBarbershop 5:7108ac9e8182 310 eL = maxError;
LtBarbershop 5:7108ac9e8182 311 }
LtBarbershop 5:7108ac9e8182 312 if (eL < -maxError);
LtBarbershop 5:7108ac9e8182 313 {
LtBarbershop 5:7108ac9e8182 314 eL = -maxError;
LtBarbershop 5:7108ac9e8182 315 }
LtBarbershop 5:7108ac9e8182 316 if (eR > maxError)
LtBarbershop 5:7108ac9e8182 317 {
LtBarbershop 5:7108ac9e8182 318 eR = maxError;
LtBarbershop 5:7108ac9e8182 319 }
LtBarbershop 5:7108ac9e8182 320 if (eR < -maxError);
LtBarbershop 5:7108ac9e8182 321 {
LtBarbershop 5:7108ac9e8182 322 eR = -maxError;
IanTheMBEDMaster 12:3058f9fb09eb 323 }
LtBarbershop 4:03bf5bdca9fb 324
LtBarbershop 4:03bf5bdca9fb 325 // accumulated error (integration)
LtBarbershop 5:7108ac9e8182 326 if (prevu1 < 1 && prevu1 > -1)
LtBarbershop 5:7108ac9e8182 327 {
LtBarbershop 5:7108ac9e8182 328 aeL += eL;
LtBarbershop 5:7108ac9e8182 329 }
LtBarbershop 5:7108ac9e8182 330 if (prevu2 < 1 && prevu2 > -1)
LtBarbershop 5:7108ac9e8182 331 {
LtBarbershop 5:7108ac9e8182 332 aeR += eR;
LtBarbershop 5:7108ac9e8182 333 }
LtBarbershop 5:7108ac9e8182 334
LtBarbershop 5:7108ac9e8182 335 // bound the accumulatd error
LtBarbershop 5:7108ac9e8182 336 if (aeL > maxAcc)
LtBarbershop 5:7108ac9e8182 337 {
LtBarbershop 5:7108ac9e8182 338 aeL = maxAcc;
LtBarbershop 5:7108ac9e8182 339 }
LtBarbershop 5:7108ac9e8182 340 if (aeL < -maxAcc)
LtBarbershop 5:7108ac9e8182 341 {
LtBarbershop 5:7108ac9e8182 342 aeL = -maxAcc;
LtBarbershop 5:7108ac9e8182 343 }
LtBarbershop 5:7108ac9e8182 344 if (aeR > maxAcc)
LtBarbershop 5:7108ac9e8182 345 {
LtBarbershop 5:7108ac9e8182 346 aeR = maxAcc;
LtBarbershop 5:7108ac9e8182 347 }
LtBarbershop 5:7108ac9e8182 348 if (aeR < -maxAcc)
LtBarbershop 5:7108ac9e8182 349 {
LtBarbershop 5:7108ac9e8182 350 aeR = -maxAcc;
LtBarbershop 5:7108ac9e8182 351 }
LtBarbershop 4:03bf5bdca9fb 352
LtBarbershop 4:03bf5bdca9fb 353 u1 = Kp*eL + Ki*aeL;
LtBarbershop 4:03bf5bdca9fb 354 u2 = Kp*eR + Ki*aeR;
LtBarbershop 5:7108ac9e8182 355
IanTheMBEDMaster 7:751d5e3e9cab 356 cSetL = userSetL + u1;
IanTheMBEDMaster 7:751d5e3e9cab 357 cSetR = userSetR + u2;
LtBarbershop 5:7108ac9e8182 358
IanTheMBEDMaster 12:3058f9fb09eb 359 // data recording code
LtBarbershop 11:521c3e8e6491 360 /*if (userSetL == 0.8f)
LtBarbershop 8:d65cba3bfc0e 361 {
LtBarbershop 8:d65cba3bfc0e 362 if (RecCount < 100)
LtBarbershop 8:d65cba3bfc0e 363 {
LtBarbershop 8:d65cba3bfc0e 364 RecX[RecCount] = aeL;
LtBarbershop 8:d65cba3bfc0e 365 RecU[RecCount] = cSetL;
LtBarbershop 8:d65cba3bfc0e 366 RecV[RecCount] = fbSpeedL;
LtBarbershop 8:d65cba3bfc0e 367 RecE[RecCount] = eL;
LtBarbershop 8:d65cba3bfc0e 368 RecCount++;
LtBarbershop 8:d65cba3bfc0e 369 }
LtBarbershop 8:d65cba3bfc0e 370 else
LtBarbershop 8:d65cba3bfc0e 371 {
LtBarbershop 8:d65cba3bfc0e 372 userSetL = 0;
LtBarbershop 8:d65cba3bfc0e 373 }
LtBarbershop 11:521c3e8e6491 374
LtBarbershop 11:521c3e8e6491 375 }*/
IanTheMBEDMaster 12:3058f9fb09eb 376
IanTheMBEDMaster 7:751d5e3e9cab 377 SetLeftMotorSpeed(cSetL);
IanTheMBEDMaster 7:751d5e3e9cab 378 SetRightMotorSpeed(cSetR);
IanTheMBEDMaster 2:1c5cc180812d 379 }
IanTheMBEDMaster 2:1c5cc180812d 380 }
LtBarbershop 1:3a40c918ff41 381
LtBarbershop 1:3a40c918ff41 382 // ******** Collision Thread ********
IanTheMBEDMaster 2:1c5cc180812d 383 void ExtCollisionThread(void const *argument)
IanTheMBEDMaster 2:1c5cc180812d 384 {
IanTheMBEDMaster 2:1c5cc180812d 385 while (1)
IanTheMBEDMaster 2:1c5cc180812d 386 {
IanTheMBEDMaster 2:1c5cc180812d 387 osSignalWait(SignalExtCollision, osWaitForever);
IanTheMBEDMaster 2:1c5cc180812d 388 led4 = !led4;
LtBarbershop 1:3a40c918ff41 389 }
LtBarbershop 1:3a40c918ff41 390 }
LtBarbershop 1:3a40c918ff41 391
LtBarbershop 1:3a40c918ff41 392 // ******** Watchdog Interrupt Handler ********
IanTheMBEDMaster 2:1c5cc180812d 393 void Watchdog(void const *n)
IanTheMBEDMaster 2:1c5cc180812d 394 {
LtBarbershop 1:3a40c918ff41 395 led3=1;
LtBarbershop 11:521c3e8e6491 396 BtS.printf("\n\r Watchdog Timeout! Oh Shit!\n\r");
LtBarbershop 1:3a40c918ff41 397 }
LtBarbershop 1:3a40c918ff41 398
LtBarbershop 1:3a40c918ff41 399 // ******** Period Timer Interrupt Handler ********
LtBarbershop 5:7108ac9e8182 400 void PiControllerISR(void)
IanTheMBEDMaster 2:1c5cc180812d 401 {
LtBarbershop 1:3a40c918ff41 402 osSignalSet(PiControl,0x1);
IanTheMBEDMaster 2:1c5cc180812d 403 }
LtBarbershop 1:3a40c918ff41 404
LtBarbershop 1:3a40c918ff41 405 // ******** Collision Interrupt Handler ********
LtBarbershop 1:3a40c918ff41 406 void ExtCollisionISR(void)
LtBarbershop 1:3a40c918ff41 407 {
LtBarbershop 1:3a40c918ff41 408 osSignalSet(ExtCollision,0x1);
LtBarbershop 1:3a40c918ff41 409 }
LtBarbershop 1:3a40c918ff41 410
IanTheMBEDMaster 12:3058f9fb09eb 411
IanTheMBEDMaster 2:1c5cc180812d 412 // --- Initialization Functions
IanTheMBEDMaster 2:1c5cc180812d 413 void InitializeSystem()
IanTheMBEDMaster 2:1c5cc180812d 414 {
IanTheMBEDMaster 2:1c5cc180812d 415 led3=0;
IanTheMBEDMaster 2:1c5cc180812d 416 led4=0;
IanTheMBEDMaster 2:1c5cc180812d 417
IanTheMBEDMaster 12:3058f9fb09eb 418 EmerStop.fall(&ExtCollisionISR); // Atach the address of the interrupt handler to the rising edge of Bumper
IanTheMBEDMaster 2:1c5cc180812d 419
IanTheMBEDMaster 2:1c5cc180812d 420 // Start execution of the Threads
IanTheMBEDMaster 2:1c5cc180812d 421 PiControl = osThreadCreate(osThread(PiControlThread), NULL);
IanTheMBEDMaster 2:1c5cc180812d 422 ExtCollision = osThreadCreate(osThread(ExtCollisionThread), NULL);
IanTheMBEDMaster 2:1c5cc180812d 423 osTimerId OneShot = osTimerCreate(osTimer(Wdtimer), osTimerOnce, (void *)0);
LtBarbershop 4:03bf5bdca9fb 424 PeriodicInt.attach(&PiControllerISR, ControlUpdate); // Specify address of the TimerISR (Ticker) function and the interval between interrupts
IanTheMBEDMaster 2:1c5cc180812d 425
IanTheMBEDMaster 6:5200ce9fa5a7 426 InitializePWM();
IanTheMBEDMaster 2:1c5cc180812d 427 InitializeEncoder();
IanTheMBEDMaster 12:3058f9fb09eb 428 BTInit();
IanTheMBEDMaster 2:1c5cc180812d 429 }
IanTheMBEDMaster 2:1c5cc180812d 430
IanTheMBEDMaster 2:1c5cc180812d 431 void InitializePWM()
IanTheMBEDMaster 2:1c5cc180812d 432 {
IanTheMBEDMaster 6:5200ce9fa5a7 433 PwmL.period(PWMPeriod);
IanTheMBEDMaster 6:5200ce9fa5a7 434 PwmR.period(PWMPeriod);
IanTheMBEDMaster 2:1c5cc180812d 435 }
IanTheMBEDMaster 2:1c5cc180812d 436
IanTheMBEDMaster 2:1c5cc180812d 437 void InitializeEncoder()
IanTheMBEDMaster 2:1c5cc180812d 438 {
IanTheMBEDMaster 2:1c5cc180812d 439 // Initialization – to be executed once (normally)
IanTheMBEDMaster 2:1c5cc180812d 440 DE0.format(16,0); // SPI format: 16-bit words, mode 0 protocol.
LtBarbershop 4:03bf5bdca9fb 441 DE0.frequency(1000000);
IanTheMBEDMaster 2:1c5cc180812d 442 SpiStart = 0;
IanTheMBEDMaster 2:1c5cc180812d 443 SpiReset = 1;
IanTheMBEDMaster 2:1c5cc180812d 444 wait_us(10);
IanTheMBEDMaster 2:1c5cc180812d 445 SpiReset = 0;
IanTheMBEDMaster 2:1c5cc180812d 446 DE0.write(0x8004); // SPI slave control word to read (only) 4-word transactions
IanTheMBEDMaster 2:1c5cc180812d 447 // starting at base address 0 within the peripheral.
IanTheMBEDMaster 2:1c5cc180812d 448 }
IanTheMBEDMaster 2:1c5cc180812d 449
IanTheMBEDMaster 12:3058f9fb09eb 450 void BTInit()
IanTheMBEDMaster 12:3058f9fb09eb 451 {
IanTheMBEDMaster 12:3058f9fb09eb 452 BtS.printf("AT+BTCANCEL\r\n");
IanTheMBEDMaster 12:3058f9fb09eb 453 BtS.printf("AT+BTSCAN\r\n");
IanTheMBEDMaster 12:3058f9fb09eb 454 }
IanTheMBEDMaster 2:1c5cc180812d 455
IanTheMBEDMaster 2:1c5cc180812d 456 // --- Other Functions
IanTheMBEDMaster 2:1c5cc180812d 457 void SetLeftMotorSpeed(float u)
LtBarbershop 1:3a40c918ff41 458 {
IanTheMBEDMaster 2:1c5cc180812d 459 float T;
IanTheMBEDMaster 2:1c5cc180812d 460 float d;
IanTheMBEDMaster 2:1c5cc180812d 461 float onTime;
IanTheMBEDMaster 2:1c5cc180812d 462
LtBarbershop 3:9a39e487b724 463 // bound the input
LtBarbershop 3:9a39e487b724 464 if (u > 1)
LtBarbershop 3:9a39e487b724 465 {
LtBarbershop 3:9a39e487b724 466 u = 1;
LtBarbershop 3:9a39e487b724 467 }
LtBarbershop 3:9a39e487b724 468
LtBarbershop 3:9a39e487b724 469 if (u < -1)
LtBarbershop 3:9a39e487b724 470 {
LtBarbershop 3:9a39e487b724 471 u = -1;
LtBarbershop 3:9a39e487b724 472 }
LtBarbershop 3:9a39e487b724 473
LtBarbershop 3:9a39e487b724 474 // calculate duty cycle timing
IanTheMBEDMaster 2:1c5cc180812d 475 T = PWMPeriod;
IanTheMBEDMaster 2:1c5cc180812d 476 d = abs(u);
IanTheMBEDMaster 2:1c5cc180812d 477 onTime = d * T;
IanTheMBEDMaster 2:1c5cc180812d 478
IanTheMBEDMaster 2:1c5cc180812d 479 PwmL.pulsewidth(onTime);
IanTheMBEDMaster 2:1c5cc180812d 480
IanTheMBEDMaster 2:1c5cc180812d 481 if (u > 0)
IanTheMBEDMaster 2:1c5cc180812d 482 {
IanTheMBEDMaster 2:1c5cc180812d 483 dirL = 1;
IanTheMBEDMaster 2:1c5cc180812d 484 }
IanTheMBEDMaster 2:1c5cc180812d 485 else
IanTheMBEDMaster 2:1c5cc180812d 486 {
IanTheMBEDMaster 2:1c5cc180812d 487 dirL = 0;
IanTheMBEDMaster 2:1c5cc180812d 488 }
IanTheMBEDMaster 2:1c5cc180812d 489 }
LtBarbershop 1:3a40c918ff41 490
IanTheMBEDMaster 2:1c5cc180812d 491 void SetRightMotorSpeed(float u)
IanTheMBEDMaster 2:1c5cc180812d 492 {
IanTheMBEDMaster 2:1c5cc180812d 493 float T;
IanTheMBEDMaster 2:1c5cc180812d 494 float d;
IanTheMBEDMaster 2:1c5cc180812d 495 float onTime;
IanTheMBEDMaster 2:1c5cc180812d 496
LtBarbershop 3:9a39e487b724 497 // bound the input
LtBarbershop 3:9a39e487b724 498 if (u > 1)
LtBarbershop 3:9a39e487b724 499 {
LtBarbershop 3:9a39e487b724 500 u = 1;
LtBarbershop 3:9a39e487b724 501 }
LtBarbershop 3:9a39e487b724 502
LtBarbershop 3:9a39e487b724 503 if (u < -1)
LtBarbershop 3:9a39e487b724 504 {
LtBarbershop 3:9a39e487b724 505 u = -1;
LtBarbershop 3:9a39e487b724 506 }
LtBarbershop 3:9a39e487b724 507
LtBarbershop 3:9a39e487b724 508 // calculate duty cycle timing
IanTheMBEDMaster 2:1c5cc180812d 509 T = PWMPeriod;
IanTheMBEDMaster 2:1c5cc180812d 510 d = abs(u);
IanTheMBEDMaster 2:1c5cc180812d 511 onTime = d * T;
IanTheMBEDMaster 2:1c5cc180812d 512
IanTheMBEDMaster 2:1c5cc180812d 513 PwmR.pulsewidth(onTime);
IanTheMBEDMaster 2:1c5cc180812d 514
IanTheMBEDMaster 2:1c5cc180812d 515 if (u > 0)
IanTheMBEDMaster 2:1c5cc180812d 516 {
IanTheMBEDMaster 2:1c5cc180812d 517 dirR = 1;
IanTheMBEDMaster 2:1c5cc180812d 518 }
IanTheMBEDMaster 2:1c5cc180812d 519 else
IanTheMBEDMaster 2:1c5cc180812d 520 {
IanTheMBEDMaster 2:1c5cc180812d 521 dirR = 0;
IanTheMBEDMaster 2:1c5cc180812d 522 }
LtBarbershop 1:3a40c918ff41 523 }
LtBarbershop 1:3a40c918ff41 524
IanTheMBEDMaster 7:751d5e3e9cab 525 void GetSpeeds()
LtBarbershop 1:3a40c918ff41 526 {
IanTheMBEDMaster 7:751d5e3e9cab 527 float leftMaxPos = 1480.0f;
IanTheMBEDMaster 7:751d5e3e9cab 528 float rightMaxPos = 1480.0f;
LtBarbershop 1:3a40c918ff41 529
IanTheMBEDMaster 6:5200ce9fa5a7 530 // Restart the SPI module each time
LtBarbershop 4:03bf5bdca9fb 531 SpiStart = 1;
LtBarbershop 4:03bf5bdca9fb 532 wait_us(5);
LtBarbershop 4:03bf5bdca9fb 533 SpiStart = 0;
LtBarbershop 4:03bf5bdca9fb 534 DE0.write(0x8004);
IanTheMBEDMaster 12:3058f9fb09eb 535
IanTheMBEDMaster 6:5200ce9fa5a7 536 // read in 4 16-bit words
LtBarbershop 4:03bf5bdca9fb 537 Var_Lock.lock();
IanTheMBEDMaster 6:5200ce9fa5a7 538 dPositionLeft = DE0.write(Dummy); // Read QEI-0 position register
IanTheMBEDMaster 6:5200ce9fa5a7 539 dTimeLeft = DE0.write(Dummy); // Read QE-0 time interval register
LtBarbershop 4:03bf5bdca9fb 540 dPositionRight = DE0.write(Dummy); // Read QEI-1 position register
LtBarbershop 4:03bf5bdca9fb 541 dTimeRight = DE0.write(Dummy); // Read QEI-1 time interval register
IanTheMBEDMaster 6:5200ce9fa5a7 542 Var_Lock.unlock();
LtBarbershop 4:03bf5bdca9fb 543
IanTheMBEDMaster 7:751d5e3e9cab 544 // calcspeed
IanTheMBEDMaster 7:751d5e3e9cab 545 fbSpeedL = ((float)dPositionLeft)/leftMaxPos;
IanTheMBEDMaster 7:751d5e3e9cab 546 fbSpeedR = ((float)dPositionRight)/rightMaxPos;
IanTheMBEDMaster 7:751d5e3e9cab 547
IanTheMBEDMaster 6:5200ce9fa5a7 548 // bound the feedback speed
IanTheMBEDMaster 7:751d5e3e9cab 549 if (fbSpeedL > 1)
IanTheMBEDMaster 6:5200ce9fa5a7 550 {
IanTheMBEDMaster 7:751d5e3e9cab 551 fbSpeedL = 1;
IanTheMBEDMaster 6:5200ce9fa5a7 552 }
IanTheMBEDMaster 7:751d5e3e9cab 553 if (fbSpeedL < -1)
IanTheMBEDMaster 6:5200ce9fa5a7 554 {
IanTheMBEDMaster 7:751d5e3e9cab 555 fbSpeedL = -1;
IanTheMBEDMaster 6:5200ce9fa5a7 556 }
IanTheMBEDMaster 7:751d5e3e9cab 557 if (fbSpeedR > 1)
IanTheMBEDMaster 6:5200ce9fa5a7 558 {
IanTheMBEDMaster 7:751d5e3e9cab 559 fbSpeedR = 1;
IanTheMBEDMaster 6:5200ce9fa5a7 560 }
IanTheMBEDMaster 7:751d5e3e9cab 561 if (fbSpeedR < -1)
IanTheMBEDMaster 6:5200ce9fa5a7 562 {
IanTheMBEDMaster 7:751d5e3e9cab 563 fbSpeedR = -1;
IanTheMBEDMaster 6:5200ce9fa5a7 564 }
LtBarbershop 9:0eb7b173d6c3 565 }
LtBarbershop 11:521c3e8e6491 566
LtBarbershop 11:521c3e8e6491 567 void IRChecker()
LtBarbershop 9:0eb7b173d6c3 568 {
LtBarbershop 9:0eb7b173d6c3 569 float IRF, IRR;
LtBarbershop 11:521c3e8e6491 570 char z;
LtBarbershop 9:0eb7b173d6c3 571 // Read Sensors
LtBarbershop 11:521c3e8e6491 572 while (z != 'q')
LtBarbershop 11:521c3e8e6491 573 {
LtBarbershop 11:521c3e8e6491 574 IRF = IRFront.read();
LtBarbershop 11:521c3e8e6491 575 IRR = IRRear.read();
LtBarbershop 11:521c3e8e6491 576 // Voltage Corresponding to nominal distance
LtBarbershop 11:521c3e8e6491 577 BtS.printf("\n\rFront Sensor: %f", IRF);
LtBarbershop 11:521c3e8e6491 578 BtS.printf("\n\rRear Sensor: %f\n\r", IRR);
LtBarbershop 11:521c3e8e6491 579 if (BtS.readable())
LtBarbershop 11:521c3e8e6491 580 {
LtBarbershop 11:521c3e8e6491 581 z = BtS.getc();
LtBarbershop 11:521c3e8e6491 582 }
LtBarbershop 11:521c3e8e6491 583 Thread::wait(1000);
LtBarbershop 11:521c3e8e6491 584 }
LtBarbershop 9:0eb7b173d6c3 585 float Nominal = 3.0;
LtBarbershop 9:0eb7b173d6c3 586 // Variable for turning robots
LtBarbershop 9:0eb7b173d6c3 587 float Turn = 0.1;
LtBarbershop 9:0eb7b173d6c3 588
LtBarbershop 9:0eb7b173d6c3 589 // Ensure Robot isn't closer than 10cm from wall (reads no voltage)
LtBarbershop 9:0eb7b173d6c3 590
LtBarbershop 11:521c3e8e6491 591 /* Compare to nominal voltage and adjust
LtBarbershop 9:0eb7b173d6c3 592 if (IRF > Nominal && IRR > Nominal)
LtBarbershop 9:0eb7b173d6c3 593 {
LtBarbershop 9:0eb7b173d6c3 594 SetR = SetR - Turn;
LtBarbershop 9:0eb7b173d6c3 595 SetL = SetL + Turn;
LtBarbershop 9:0eb7b173d6c3 596 SetRightMotorSpeed(SetR);
LtBarbershop 9:0eb7b173d6c3 597 SetLeftMotorSpeed(SetL);
LtBarbershop 9:0eb7b173d6c3 598 }
LtBarbershop 9:0eb7b173d6c3 599 if (IRF < Nominal && IRR < Nominal
LtBarbershop 9:0eb7b173d6c3 600 {
LtBarbershop 9:0eb7b173d6c3 601 SetR = SetR - Turn;
LtBarbershop 9:0eb7b173d6c3 602 SetL = SetL + Turn;
LtBarbershop 9:0eb7b173d6c3 603 SetRightMotorSpeed(SetR);
LtBarbershop 9:0eb7b173d6c3 604 SetLeftMotorSpeed(SetL);
LtBarbershop 9:0eb7b173d6c3 605 }
LtBarbershop 9:0eb7b173d6c3 606
LtBarbershop 9:0eb7b173d6c3 607 // Compare rangers to each other
LtBarbershop 9:0eb7b173d6c3 608 if (IRF > IRR) // IRF closer than IRR
LtBarbershop 9:0eb7b173d6c3 609 {
LtBarbershop 9:0eb7b173d6c3 610 SetR = SetR + Turn;
LtBarbershop 9:0eb7b173d6c3 611 SetL = SetL - Turn;
LtBarbershop 9:0eb7b173d6c3 612 SetRightMotorSpeed(SetR);
LtBarbershop 9:0eb7b173d6c3 613 SetLeftMotorSpeed(SetL);
LtBarbershop 9:0eb7b173d6c3 614 }
LtBarbershop 9:0eb7b173d6c3 615 if (IRF < IRR) // IRF further than IRR
LtBarbershop 9:0eb7b173d6c3 616 {
LtBarbershop 9:0eb7b173d6c3 617 SetR = SetR - Turn;
LtBarbershop 9:0eb7b173d6c3 618 SetL = SetL + Turn;
LtBarbershop 9:0eb7b173d6c3 619 SetRightMotorSpeed(SetR);
LtBarbershop 9:0eb7b173d6c3 620 SetLeftMotorSpeed(SetL);
LtBarbershop 9:0eb7b173d6c3 621 }
LtBarbershop 11:521c3e8e6491 622 */
LtBarbershop 11:521c3e8e6491 623 }
LtBarbershop 10:a3ecedc8d9d7 624
LtBarbershop 11:521c3e8e6491 625 void DisplayMenu()
LtBarbershop 11:521c3e8e6491 626 {
IanTheMBEDMaster 12:3058f9fb09eb 627 BtS.printf("\r\n\nPress the corresponding key to do something:\r\n");
LtBarbershop 11:521c3e8e6491 628 BtS.printf("| Key | Action\n\r");
IanTheMBEDMaster 12:3058f9fb09eb 629 BtS.printf("|-----|----------------------------\n\r");
LtBarbershop 11:521c3e8e6491 630 BtS.printf("| d | Drive the robot using wasd keys\n\r");
LtBarbershop 11:521c3e8e6491 631 BtS.printf("| w | Robot performs wall following\n\r");
LtBarbershop 11:521c3e8e6491 632 BtS.printf("| 0 | Debug mode\n\r");
IanTheMBEDMaster 12:3058f9fb09eb 633 BtS.printf("| q | Quit current action, stop the robot, and return to this menu\n\r\n");
LtBarbershop 11:521c3e8e6491 634 }
LtBarbershop 11:521c3e8e6491 635
LtBarbershop 11:521c3e8e6491 636 void Ramp(float speed, unsigned int time, unsigned short motor)
LtBarbershop 11:521c3e8e6491 637 {
LtBarbershop 11:521c3e8e6491 638 const unsigned short steps = 20;
LtBarbershop 11:521c3e8e6491 639 float changeL = (speed - userSetL)/steps;
LtBarbershop 11:521c3e8e6491 640 float changeR = (speed - userSetR)/steps;
LtBarbershop 11:521c3e8e6491 641 unsigned short i;
LtBarbershop 11:521c3e8e6491 642 // calculate wait time (we wont worry too much about rounding errors)
LtBarbershop 11:521c3e8e6491 643 unsigned int waitTime = time/steps;
LtBarbershop 11:521c3e8e6491 644
LtBarbershop 11:521c3e8e6491 645 for (i = 0; i < steps; i++)
LtBarbershop 11:521c3e8e6491 646 {
LtBarbershop 11:521c3e8e6491 647 //Thread::wait(200);
LtBarbershop 11:521c3e8e6491 648 Thread::wait(waitTime);
LtBarbershop 11:521c3e8e6491 649
LtBarbershop 11:521c3e8e6491 650 if (motor == 0)
LtBarbershop 11:521c3e8e6491 651 {
LtBarbershop 11:521c3e8e6491 652 // change both speeds
LtBarbershop 11:521c3e8e6491 653 userSetL += changeL;
LtBarbershop 11:521c3e8e6491 654 userSetR += changeR;
LtBarbershop 11:521c3e8e6491 655 continue;
LtBarbershop 11:521c3e8e6491 656 }
LtBarbershop 11:521c3e8e6491 657 if (motor == 1)
LtBarbershop 11:521c3e8e6491 658 {
LtBarbershop 11:521c3e8e6491 659 userSetL += changeL;
LtBarbershop 11:521c3e8e6491 660 continue;
LtBarbershop 11:521c3e8e6491 661 }
LtBarbershop 11:521c3e8e6491 662 if (motor == 2)
LtBarbershop 11:521c3e8e6491 663 {
LtBarbershop 11:521c3e8e6491 664 userSetR += changeR;
IanTheMBEDMaster 12:3058f9fb09eb 665 }
IanTheMBEDMaster 12:3058f9fb09eb 666 }
LtBarbershop 9:0eb7b173d6c3 667 }