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@7:751d5e3e9cab, 2013-02-27 (annotated)

Committer:
IanTheMBEDMaster
Date:
Wed Feb 27 21:46:23 2013 +0000
Revision:
7:751d5e3e9cab
Parent:
6:5200ce9fa5a7
Child:
8:d65cba3bfc0e
PI WORKS!!

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