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@2:1c5cc180812d, 2013-02-15 (annotated)

Committer:
IanTheMBEDMaster
Date:
Fri Feb 15 18:46:11 2013 +0000
Revision:
2:1c5cc180812d
Parent:
1:3a40c918ff41
Child:
3:9a39e487b724
Cleaned up the code and added SPI

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 2:1c5cc180812d 10 #define PWMPeriod 0.001
LtBarbershop 1:3a40c918ff41 11
IanTheMBEDMaster 2:1c5cc180812d 12 // --- Function prototypes
LtBarbershop 1:3a40c918ff41 13 void PiControllerISR(void);
LtBarbershop 1:3a40c918ff41 14 void WdtFaultISR(void);
LtBarbershop 1:3a40c918ff41 15 void ExtCollisionISR(void);
LtBarbershop 1:3a40c918ff41 16 void PiControlThread(void const *argument);
LtBarbershop 1:3a40c918ff41 17 void ExtCollisionThread(void const *argument);
LtBarbershop 1:3a40c918ff41 18 void Watchdog(void const *n);
IanTheMBEDMaster 2:1c5cc180812d 19 void InitializeSystem();
IanTheMBEDMaster 2:1c5cc180812d 20 void InitializeEncoder();
IanTheMBEDMaster 2:1c5cc180812d 21 void InitializePWM();
IanTheMBEDMaster 2:1c5cc180812d 22 void PwmSetOut(float d, float T);
IanTheMBEDMaster 2:1c5cc180812d 23 void ReadEncoder();
IanTheMBEDMaster 2:1c5cc180812d 24 void SetLeftMotorSpeed(float u);
IanTheMBEDMaster 2:1c5cc180812d 25 void SetRightMotorSpeed(float u);
LtBarbershop 1:3a40c918ff41 26
LtBarbershop 1:3a40c918ff41 27 // Global variables for interrupt handler
LtBarbershop 1:3a40c918ff41 28 float u1;
LtBarbershop 1:3a40c918ff41 29 float u2;
IanTheMBEDMaster 2:1c5cc180812d 30
IanTheMBEDMaster 2:1c5cc180812d 31 // --- Processes and threads
LtBarbershop 1:3a40c918ff41 32 int32_t SignalPi, SignalWdt, SignalExtCollision;
LtBarbershop 1:3a40c918ff41 33 osThreadId PiControl,WdtFault,ExtCollision;
LtBarbershop 1:3a40c918ff41 34 osThreadDef(PiControlThread, osPriorityNormal, DEFAULT_STACK_SIZE);
LtBarbershop 1:3a40c918ff41 35 osThreadDef(ExtCollisionThread, osPriorityNormal, DEFAULT_STACK_SIZE);
LtBarbershop 1:3a40c918ff41 36 osTimerDef(Wdtimer, Watchdog);
LtBarbershop 1:3a40c918ff41 37
IanTheMBEDMaster 2:1c5cc180812d 38 /* PIN-OUT
IanTheMBEDMaster 2:1c5cc180812d 39
IanTheMBEDMaster 2:1c5cc180812d 40 MOSI Quad Enc 5|-|
IanTheMBEDMaster 2:1c5cc180812d 41 MISO Quad Enc 6|-|
IanTheMBEDMaster 2:1c5cc180812d 42 SCK Quad Enc 7|-|
IanTheMBEDMaster 2:1c5cc180812d 43 SPI Start Quad E 8|-|
IanTheMBEDMaster 2:1c5cc180812d 44 SPI Reset Quad E 9|-|
IanTheMBEDMaster 2:1c5cc180812d 45
IanTheMBEDMaster 2:1c5cc180812d 46 Bluetooth tx 13|-|28
IanTheMBEDMaster 2:1c5cc180812d 47 Bluetooth rx 14|-|27
IanTheMBEDMaster 2:1c5cc180812d 48 15|-|26 Brake, Right Motor, M2
IanTheMBEDMaster 2:1c5cc180812d 49 16|-|25 Dir, Right Motor, M2
IanTheMBEDMaster 2:1c5cc180812d 50 17|-|24 PWM, Right Motor, M2
IanTheMBEDMaster 2:1c5cc180812d 51 18|-|23 Brake, Left Motor, M1
IanTheMBEDMaster 2:1c5cc180812d 52 19|-|22 Dir, Left Motor, M1
IanTheMBEDMaster 2:1c5cc180812d 53 20|-|21 PWM, Left Motor, M1
IanTheMBEDMaster 2:1c5cc180812d 54 */
IanTheMBEDMaster 2:1c5cc180812d 55
IanTheMBEDMaster 2:1c5cc180812d 56 // --- IO Port Configuration
LtBarbershop 1:3a40c918ff41 57 DigitalOut led1(LED1);
LtBarbershop 1:3a40c918ff41 58 DigitalOut led2(LED2);
LtBarbershop 1:3a40c918ff41 59 DigitalOut led3(LED3);
LtBarbershop 1:3a40c918ff41 60 DigitalOut led4(LED4);
LtBarbershop 1:3a40c918ff41 61 DigitalOut dirL(p22);
IanTheMBEDMaster 2:1c5cc180812d 62 DigitalOut brakeL(p23);
IanTheMBEDMaster 2:1c5cc180812d 63 PwmOut PwmL(p21);
IanTheMBEDMaster 2:1c5cc180812d 64 DigitalOut dirR(p25);
IanTheMBEDMaster 2:1c5cc180812d 65 DigitalOut brakeR(p26);
IanTheMBEDMaster 2:1c5cc180812d 66 PwmOut PwmR(p24);
IanTheMBEDMaster 2:1c5cc180812d 67 Serial BluetoothSerial(p13, p14); // (tx, rx) for PC serial channel
LtBarbershop 1:3a40c918ff41 68 Serial pc(USBTX, USBRX); // (tx, rx) for Parani/Promi Bluetooth serial channel
IanTheMBEDMaster 2:1c5cc180812d 69 SPI DE0(p5, p6, p7); // (mosi, miso, sclk) DE0 is the SPI channel with the DE0 FPGA
IanTheMBEDMaster 2:1c5cc180812d 70 DigitalOut SpiReset(p9); // Reset for all devices within the slave SPI peripheral in the DE0 FPGA
IanTheMBEDMaster 2:1c5cc180812d 71 DigitalOut SpiStart(p8); // Places SPI interace on the DE0 FPGA into control mode
IanTheMBEDMaster 2:1c5cc180812d 72 InterruptIn Bumper(p10); // External interrupt pin
LtBarbershop 1:3a40c918ff41 73
LtBarbershop 1:3a40c918ff41 74 Ticker PeriodicInt;
IanTheMBEDMaster 2:1c5cc180812d 75
LtBarbershop 1:3a40c918ff41 76
LtBarbershop 1:3a40c918ff41 77 // ******** Main Thread ********
IanTheMBEDMaster 2:1c5cc180812d 78 int main()
IanTheMBEDMaster 2:1c5cc180812d 79 {
IanTheMBEDMaster 2:1c5cc180812d 80 InitializeSystem();
IanTheMBEDMaster 2:1c5cc180812d 81
IanTheMBEDMaster 2:1c5cc180812d 82 pc.printf("\r\n Robot Initialization Complete \r\n");
IanTheMBEDMaster 2:1c5cc180812d 83
IanTheMBEDMaster 2:1c5cc180812d 84 BluetoothSerial.printf("\n\n\rTap w-a-s-d keys for differential speed control: ");
IanTheMBEDMaster 2:1c5cc180812d 85
IanTheMBEDMaster 2:1c5cc180812d 86 while(1)
IanTheMBEDMaster 2:1c5cc180812d 87 {
LtBarbershop 1:3a40c918ff41 88
IanTheMBEDMaster 2:1c5cc180812d 89 /*if (pc.readable()){
IanTheMBEDMaster 2:1c5cc180812d 90 x=pc.getc();
IanTheMBEDMaster 2:1c5cc180812d 91 pc.putc(x); //Echo keyboard entry
IanTheMBEDMaster 2:1c5cc180812d 92 osTimerStart(OneShot, 2000); // Set the watchdog timer interrupt to 2s.
IanTheMBEDMaster 2:1c5cc180812d 93
IanTheMBEDMaster 2:1c5cc180812d 94 }*/
IanTheMBEDMaster 2:1c5cc180812d 95 if(pc.readable())
LtBarbershop 1:3a40c918ff41 96 {
IanTheMBEDMaster 2:1c5cc180812d 97 pc.printf("\n\r Enter a motor speed (with sign as direction:\n\r");
IanTheMBEDMaster 2:1c5cc180812d 98 pc.scanf("%f", &u1);
IanTheMBEDMaster 2:1c5cc180812d 99 pc.printf("%f", u1);
IanTheMBEDMaster 2:1c5cc180812d 100 /* x=pc.getc();
IanTheMBEDMaster 2:1c5cc180812d 101 if(x=='w')
LtBarbershop 1:3a40c918ff41 102 {
IanTheMBEDMaster 2:1c5cc180812d 103 // increase motor speed
IanTheMBEDMaster 2:1c5cc180812d 104 u1 += 0.02;
IanTheMBEDMaster 2:1c5cc180812d 105 if (u1 > 1)
IanTheMBEDMaster 2:1c5cc180812d 106 {
IanTheMBEDMaster 2:1c5cc180812d 107 u1 = 1;
IanTheMBEDMaster 2:1c5cc180812d 108 }
LtBarbershop 1:3a40c918ff41 109 }
IanTheMBEDMaster 2:1c5cc180812d 110 else if(x=='s')
LtBarbershop 1:3a40c918ff41 111 {
IanTheMBEDMaster 2:1c5cc180812d 112 // u1ecrease motor speed
IanTheMBEDMaster 2:1c5cc180812d 113 u1 -= 0.02;
IanTheMBEDMaster 2:1c5cc180812d 114 if (u1 < 0)
IanTheMBEDMaster 2:1c5cc180812d 115 {
IanTheMBEDMaster 2:1c5cc180812d 116 u1 = 0;
IanTheMBEDMaster 2:1c5cc180812d 117 }
LtBarbershop 1:3a40c918ff41 118 }
IanTheMBEDMaster 2:1c5cc180812d 119 //else if(x=='a') ...
IanTheMBEDMaster 2:1c5cc180812d 120 //else if(x=='d') ...
IanTheMBEDMaster 2:1c5cc180812d 121 */
IanTheMBEDMaster 2:1c5cc180812d 122 if (u1 > 1)
IanTheMBEDMaster 2:1c5cc180812d 123 {
IanTheMBEDMaster 2:1c5cc180812d 124 u1 = 1;
IanTheMBEDMaster 2:1c5cc180812d 125 }
IanTheMBEDMaster 2:1c5cc180812d 126
IanTheMBEDMaster 2:1c5cc180812d 127 if (u1 < -1)
IanTheMBEDMaster 2:1c5cc180812d 128 {
IanTheMBEDMaster 2:1c5cc180812d 129 u1 = -1;
IanTheMBEDMaster 2:1c5cc180812d 130 }
IanTheMBEDMaster 2:1c5cc180812d 131 }
IanTheMBEDMaster 2:1c5cc180812d 132
IanTheMBEDMaster 2:1c5cc180812d 133 Thread::wait(500); // Wait 500 ms
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 2:1c5cc180812d 145 // Read encoder and display results
IanTheMBEDMaster 2:1c5cc180812d 146 ReadEncoder();
IanTheMBEDMaster 2:1c5cc180812d 147
IanTheMBEDMaster 2:1c5cc180812d 148 SetLeftMotorSpeed(u1);
IanTheMBEDMaster 2:1c5cc180812d 149 SetRightMotorSpeed(u2);
LtBarbershop 1:3a40c918ff41 150
IanTheMBEDMaster 2:1c5cc180812d 151 }
IanTheMBEDMaster 2:1c5cc180812d 152 }
LtBarbershop 1:3a40c918ff41 153
LtBarbershop 1:3a40c918ff41 154 // ******** Collision Thread ********
IanTheMBEDMaster 2:1c5cc180812d 155 void ExtCollisionThread(void const *argument)
IanTheMBEDMaster 2:1c5cc180812d 156 {
IanTheMBEDMaster 2:1c5cc180812d 157 while (1)
IanTheMBEDMaster 2:1c5cc180812d 158 {
IanTheMBEDMaster 2:1c5cc180812d 159 osSignalWait(SignalExtCollision, osWaitForever);
IanTheMBEDMaster 2:1c5cc180812d 160 led4 = !led4;
LtBarbershop 1:3a40c918ff41 161 }
LtBarbershop 1:3a40c918ff41 162 }
LtBarbershop 1:3a40c918ff41 163
LtBarbershop 1:3a40c918ff41 164 // ******** Watchdog Interrupt Handler ********
IanTheMBEDMaster 2:1c5cc180812d 165 void Watchdog(void const *n)
IanTheMBEDMaster 2:1c5cc180812d 166 {
LtBarbershop 1:3a40c918ff41 167 led3=1;
LtBarbershop 1:3a40c918ff41 168 }
LtBarbershop 1:3a40c918ff41 169
LtBarbershop 1:3a40c918ff41 170 // ******** Period Timer Interrupt Handler ********
IanTheMBEDMaster 2:1c5cc180812d 171 void PiControllerISR(void)
IanTheMBEDMaster 2:1c5cc180812d 172 {
LtBarbershop 1:3a40c918ff41 173 osSignalSet(PiControl,0x1);
IanTheMBEDMaster 2:1c5cc180812d 174 }
LtBarbershop 1:3a40c918ff41 175
LtBarbershop 1:3a40c918ff41 176 // ******** Collision Interrupt Handler ********
LtBarbershop 1:3a40c918ff41 177 void ExtCollisionISR(void)
LtBarbershop 1:3a40c918ff41 178 {
LtBarbershop 1:3a40c918ff41 179 osSignalSet(ExtCollision,0x1);
LtBarbershop 1:3a40c918ff41 180 }
LtBarbershop 1:3a40c918ff41 181
IanTheMBEDMaster 2:1c5cc180812d 182 // --- Initialization Functions
IanTheMBEDMaster 2:1c5cc180812d 183 void InitializeSystem()
IanTheMBEDMaster 2:1c5cc180812d 184 {
IanTheMBEDMaster 2:1c5cc180812d 185 led3=0;
IanTheMBEDMaster 2:1c5cc180812d 186 led4=0;
IanTheMBEDMaster 2:1c5cc180812d 187
IanTheMBEDMaster 2:1c5cc180812d 188 Bumper.rise(&ExtCollisionISR); // Atach the address of the interrupt handler to the rising edge of Bumper
IanTheMBEDMaster 2:1c5cc180812d 189
IanTheMBEDMaster 2:1c5cc180812d 190 // Start execution of the Threads
IanTheMBEDMaster 2:1c5cc180812d 191 PiControl = osThreadCreate(osThread(PiControlThread), NULL);
IanTheMBEDMaster 2:1c5cc180812d 192 ExtCollision = osThreadCreate(osThread(ExtCollisionThread), NULL);
IanTheMBEDMaster 2:1c5cc180812d 193 osTimerId OneShot = osTimerCreate(osTimer(Wdtimer), osTimerOnce, (void *)0);
IanTheMBEDMaster 2:1c5cc180812d 194 PeriodicInt.attach(&PiControllerISR, .02); // Specify address of the TimerISR (Ticker) function and the interval between interrupts
IanTheMBEDMaster 2:1c5cc180812d 195
IanTheMBEDMaster 2:1c5cc180812d 196 InitializeEncoder();
IanTheMBEDMaster 2:1c5cc180812d 197 }
IanTheMBEDMaster 2:1c5cc180812d 198
IanTheMBEDMaster 2:1c5cc180812d 199 void InitializePWM()
IanTheMBEDMaster 2:1c5cc180812d 200 {
IanTheMBEDMaster 2:1c5cc180812d 201
IanTheMBEDMaster 2:1c5cc180812d 202 }
IanTheMBEDMaster 2:1c5cc180812d 203
IanTheMBEDMaster 2:1c5cc180812d 204 void InitializeEncoder()
IanTheMBEDMaster 2:1c5cc180812d 205 {
IanTheMBEDMaster 2:1c5cc180812d 206 // Initialization – to be executed once (normally)
IanTheMBEDMaster 2:1c5cc180812d 207 DE0.format(16,0); // SPI format: 16-bit words, mode 0 protocol.
IanTheMBEDMaster 2:1c5cc180812d 208 SpiStart = 0;
IanTheMBEDMaster 2:1c5cc180812d 209 SpiReset = 1;
IanTheMBEDMaster 2:1c5cc180812d 210 wait_us(10);
IanTheMBEDMaster 2:1c5cc180812d 211 SpiReset = 0;
IanTheMBEDMaster 2:1c5cc180812d 212 DE0.write(0x8004); // SPI slave control word to read (only) 4-word transactions
IanTheMBEDMaster 2:1c5cc180812d 213 // starting at base address 0 within the peripheral.
IanTheMBEDMaster 2:1c5cc180812d 214 }
IanTheMBEDMaster 2:1c5cc180812d 215
IanTheMBEDMaster 2:1c5cc180812d 216
IanTheMBEDMaster 2:1c5cc180812d 217 // --- Other Functions
IanTheMBEDMaster 2:1c5cc180812d 218 void SetLeftMotorSpeed(float u)
LtBarbershop 1:3a40c918ff41 219 {
IanTheMBEDMaster 2:1c5cc180812d 220 float T;
IanTheMBEDMaster 2:1c5cc180812d 221 float d;
IanTheMBEDMaster 2:1c5cc180812d 222 float onTime;
IanTheMBEDMaster 2:1c5cc180812d 223
IanTheMBEDMaster 2:1c5cc180812d 224 T = PWMPeriod;
IanTheMBEDMaster 2:1c5cc180812d 225 d = abs(u);
IanTheMBEDMaster 2:1c5cc180812d 226 onTime = d * T;
IanTheMBEDMaster 2:1c5cc180812d 227
IanTheMBEDMaster 2:1c5cc180812d 228 PwmL.period(T);
IanTheMBEDMaster 2:1c5cc180812d 229 PwmL.pulsewidth(onTime);
IanTheMBEDMaster 2:1c5cc180812d 230
IanTheMBEDMaster 2:1c5cc180812d 231 if (u > 0)
IanTheMBEDMaster 2:1c5cc180812d 232 {
IanTheMBEDMaster 2:1c5cc180812d 233 dirL = 1;
IanTheMBEDMaster 2:1c5cc180812d 234 }
IanTheMBEDMaster 2:1c5cc180812d 235 else
IanTheMBEDMaster 2:1c5cc180812d 236 {
IanTheMBEDMaster 2:1c5cc180812d 237 dirL = 0;
IanTheMBEDMaster 2:1c5cc180812d 238 }
IanTheMBEDMaster 2:1c5cc180812d 239 }
LtBarbershop 1:3a40c918ff41 240
IanTheMBEDMaster 2:1c5cc180812d 241 void SetRightMotorSpeed(float u)
IanTheMBEDMaster 2:1c5cc180812d 242 {
IanTheMBEDMaster 2:1c5cc180812d 243 float T;
IanTheMBEDMaster 2:1c5cc180812d 244 float d;
IanTheMBEDMaster 2:1c5cc180812d 245 float onTime;
IanTheMBEDMaster 2:1c5cc180812d 246
IanTheMBEDMaster 2:1c5cc180812d 247 T = PWMPeriod;
IanTheMBEDMaster 2:1c5cc180812d 248 d = abs(u);
IanTheMBEDMaster 2:1c5cc180812d 249 onTime = d * T;
IanTheMBEDMaster 2:1c5cc180812d 250
IanTheMBEDMaster 2:1c5cc180812d 251 PwmR.period(T);
IanTheMBEDMaster 2:1c5cc180812d 252 PwmR.pulsewidth(onTime);
IanTheMBEDMaster 2:1c5cc180812d 253
IanTheMBEDMaster 2:1c5cc180812d 254 if (u > 0)
IanTheMBEDMaster 2:1c5cc180812d 255 {
IanTheMBEDMaster 2:1c5cc180812d 256 dirR = 1;
IanTheMBEDMaster 2:1c5cc180812d 257 }
IanTheMBEDMaster 2:1c5cc180812d 258 else
IanTheMBEDMaster 2:1c5cc180812d 259 {
IanTheMBEDMaster 2:1c5cc180812d 260 dirR = 0;
IanTheMBEDMaster 2:1c5cc180812d 261 }
LtBarbershop 1:3a40c918ff41 262 }
LtBarbershop 1:3a40c918ff41 263
LtBarbershop 1:3a40c918ff41 264 void ReadEncoder()
LtBarbershop 1:3a40c918ff41 265 {
LtBarbershop 1:3a40c918ff41 266 int dPositionRight, dTimeRight, dPositionLeft, dTimeLeft;
LtBarbershop 1:3a40c918ff41 267
LtBarbershop 1:3a40c918ff41 268 // May be executed in a loop
LtBarbershop 1:3a40c918ff41 269 dPositionRight = DE0.write(Dummy); // Read QEI-0 position register
LtBarbershop 1:3a40c918ff41 270 dTimeRight = DE0.write(Dummy); // Read QE-0 time interval register
LtBarbershop 1:3a40c918ff41 271 dPositionLeft = DE0.write(Dummy); // Read QEI-1 position register
LtBarbershop 1:3a40c918ff41 272 dTimeLeft = DE0.write(Dummy); // Read QEI-1 time interval register
IanTheMBEDMaster 2:1c5cc180812d 273
IanTheMBEDMaster 2:1c5cc180812d 274 // simply write out the results for now
IanTheMBEDMaster 2:1c5cc180812d 275 pc.printf("Left Position: %d \n\r", dPositionLeft);
IanTheMBEDMaster 2:1c5cc180812d 276 pc.printf("Left Time: %d \n\r", dTimeLeft);
IanTheMBEDMaster 2:1c5cc180812d 277 pc.printf("Right Position: %d \n\r", dPositionRight);
IanTheMBEDMaster 2:1c5cc180812d 278 pc.printf("Right Time: %d \n\n\r", dTimeRight);
IanTheMBEDMaster 2:1c5cc180812d 279 }