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@4:03bf5bdca9fb, 2013-02-22 (annotated)

Committer:
LtBarbershop
Date:
Fri Feb 22 20:57:43 2013 +0000
Revision:
4:03bf5bdca9fb
Parent:
3:9a39e487b724
Child:
5:7108ac9e8182
Feb 22 - End of day

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