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@10:a3ecedc8d9d7, 2013-03-12 (annotated)

Committer:
LtBarbershop
Date:
Tue Mar 12 19:40:04 2013 +0000
Revision:
10:a3ecedc8d9d7
Parent:
9:0eb7b173d6c3
Child:
11:521c3e8e6491
March 12, Bluetooth functional and WASD keys used added

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