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@6:5200ce9fa5a7, 2013-02-26 (annotated)

Committer:
IanTheMBEDMaster
Date:
Tue Feb 26 19:58:06 2013 +0000
Revision:
6:5200ce9fa5a7
Parent:
5:7108ac9e8182
Child:
7:751d5e3e9cab
Created direction recognition and cleaned up some code.

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