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@9:0eb7b173d6c3, 2013-03-11 (annotated)

Committer:
LtBarbershop
Date:
Mon Mar 11 14:25:40 2013 +0000
Revision:
9:0eb7b173d6c3
Parent:
8:d65cba3bfc0e
Child:
10:a3ecedc8d9d7
March 11, 2013 - added basic framework for the IR Ranger.; - Defined Pins; - Created Function; - Created Prototype; - Added Basic Logic

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