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@8:d65cba3bfc0e, 2013-03-01 (annotated)

Committer:
LtBarbershop
Date:
Fri Mar 01 20:58:03 2013 +0000
Revision:
8:d65cba3bfc0e
Parent:
7:751d5e3e9cab
Child:
9:0eb7b173d6c3
Work from March 1, 5 PM

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