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@5:7108ac9e8182, 2013-02-25 (annotated)

Committer:
LtBarbershop
Date:
Mon Feb 25 21:07:42 2013 +0000
Revision:
5:7108ac9e8182
Parent:
4:03bf5bdca9fb
Child:
6:5200ce9fa5a7
Feb 25 - TODO: write encoder code to determine direction

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