Thomas Elliott / Mbed 2 deprecated Project

NUMBA 1 Robotics Project group

Dependencies:   mbed-rtos mbed

main.cpp@1:3a40c918ff41, 2013-02-13 (annotated)

Committer:
LtBarbershop
Date:
Wed Feb 13 21:08:05 2013 +0000
Revision:
1:3a40c918ff41
Parent:
0:6321191f814a 
feb 13 updates

Who changed what in which revision?

UserRevisionLine numberNew contents of line
LtBarbershop 1:3a40c918ff41 1 #include "mbed.h"
LtBarbershop 1:3a40c918ff41 2 #include "rtos.h"
LtBarbershop 1:3a40c918ff41 3
LtBarbershop 1:3a40c918ff41 4
LtBarbershop 1:3a40c918ff41 5 // C.P. Diduch
LtBarbershop 1:3a40c918ff41 6 // EE4333 Robotics Lab-3
LtBarbershop 1:3a40c918ff41 7 // Implementation of a PI Speed Control System
LtBarbershop 1:3a40c918ff41 8 // December 17, 2012.
LtBarbershop 1:3a40c918ff41 9
LtBarbershop 1:3a40c918ff41 10 #define Dummy 0
LtBarbershop 1:3a40c918ff41 11
LtBarbershop 1:3a40c918ff41 12
LtBarbershop 1:3a40c918ff41 13 // Function prototypes
LtBarbershop 1:3a40c918ff41 14 void PiControllerISR(void);
LtBarbershop 1:3a40c918ff41 15 void WdtFaultISR(void);
LtBarbershop 1:3a40c918ff41 16 void ExtCollisionISR(void);
LtBarbershop 1:3a40c918ff41 17 void PiControlThread(void const *argument);
LtBarbershop 1:3a40c918ff41 18 void ExtCollisionThread(void const *argument);
LtBarbershop 1:3a40c918ff41 19 void Watchdog(void const *n);
LtBarbershop 1:3a40c918ff41 20
LtBarbershop 1:3a40c918ff41 21 // Global variables for interrupt handler
LtBarbershop 1:3a40c918ff41 22 float u1;
LtBarbershop 1:3a40c918ff41 23 float u2;
LtBarbershop 1:3a40c918ff41 24 // Processes and threads
LtBarbershop 1:3a40c918ff41 25 int32_t SignalPi, SignalWdt, SignalExtCollision;
LtBarbershop 1:3a40c918ff41 26 osThreadId PiControl,WdtFault,ExtCollision;
LtBarbershop 1:3a40c918ff41 27 osThreadDef(PiControlThread, osPriorityNormal, DEFAULT_STACK_SIZE);
LtBarbershop 1:3a40c918ff41 28 osThreadDef(ExtCollisionThread, osPriorityNormal, DEFAULT_STACK_SIZE);
LtBarbershop 1:3a40c918ff41 29 osTimerDef(Wdtimer, Watchdog);
LtBarbershop 1:3a40c918ff41 30
LtBarbershop 1:3a40c918ff41 31 // IO Port Configuration
LtBarbershop 1:3a40c918ff41 32 DigitalOut led1(LED1);
LtBarbershop 1:3a40c918ff41 33 DigitalOut led2(LED2);
LtBarbershop 1:3a40c918ff41 34 DigitalOut led3(LED3);
LtBarbershop 1:3a40c918ff41 35 DigitalOut led4(LED4);
LtBarbershop 1:3a40c918ff41 36 DigitalOut dirL(p22);
LtBarbershop 1:3a40c918ff41 37
LtBarbershop 1:3a40c918ff41 38 Serial BluetoothSerial(p28, p27); // (tx, rx) for PC serial channel
LtBarbershop 1:3a40c918ff41 39 Serial pc(USBTX, USBRX); // (tx, rx) for Parani/Promi Bluetooth serial channel
LtBarbershop 1:3a40c918ff41 40
LtBarbershop 1:3a40c918ff41 41 // Prototypes
LtBarbershop 1:3a40c918ff41 42 void PwmSetOut(float d, float T);
LtBarbershop 1:3a40c918ff41 43 void ReadEncoder();
LtBarbershop 1:3a40c918ff41 44 void InitializeEncoder();
LtBarbershop 1:3a40c918ff41 45
LtBarbershop 1:3a40c918ff41 46 Ticker PeriodicInt;
LtBarbershop 1:3a40c918ff41 47 SPI DE0(p5, p6, p7); // (mosi, miso, sclk) DE0 is the SPI channel with the DE0 FPGA
LtBarbershop 1:3a40c918ff41 48 DigitalOut SpiReset(p11); // Reset for all devices within the slave SPI peripheral in the DE0 FPGA
LtBarbershop 1:3a40c918ff41 49 DigitalOut SpiStart(p12); // Places SPI interace on the DE0 FPGA into control mode
LtBarbershop 1:3a40c918ff41 50 DigitalOut dir1(p22);
LtBarbershop 1:3a40c918ff41 51
LtBarbershop 1:3a40c918ff41 52 // ******** Main Thread ********
LtBarbershop 1:3a40c918ff41 53 int main() {
LtBarbershop 1:3a40c918ff41 54 char x;
LtBarbershop 1:3a40c918ff41 55 char string[30];
LtBarbershop 1:3a40c918ff41 56
LtBarbershop 1:3a40c918ff41 57 //float d = 0.1;
LtBarbershop 1:3a40c918ff41 58 //float T = 0.001;
LtBarbershop 1:3a40c918ff41 59
LtBarbershop 1:3a40c918ff41 60 led3=0;
LtBarbershop 1:3a40c918ff41 61 led4=0;
LtBarbershop 1:3a40c918ff41 62
LtBarbershop 1:3a40c918ff41 63 InterruptIn Bumper(p8); // External interrupt pin
LtBarbershop 1:3a40c918ff41 64 Bumper.rise(&ExtCollisionISR); // Atach the address of the interrupt handler to the rising edge of Bumper
LtBarbershop 1:3a40c918ff41 65
LtBarbershop 1:3a40c918ff41 66 // Start execution of the Threads
LtBarbershop 1:3a40c918ff41 67 PiControl = osThreadCreate(osThread(PiControlThread), NULL);
LtBarbershop 1:3a40c918ff41 68 ExtCollision = osThreadCreate(osThread(ExtCollisionThread), NULL);
LtBarbershop 1:3a40c918ff41 69 osTimerId OneShot = osTimerCreate(osTimer(Wdtimer), osTimerOnce, (void *)0);
LtBarbershop 1:3a40c918ff41 70
LtBarbershop 1:3a40c918ff41 71 pc.printf("\r\n RTOS Template: \r\n");
LtBarbershop 1:3a40c918ff41 72 SpiStart=0;
LtBarbershop 1:3a40c918ff41 73 SpiReset=1;
LtBarbershop 1:3a40c918ff41 74 wait_us(10);
LtBarbershop 1:3a40c918ff41 75 SpiReset=0;
LtBarbershop 1:3a40c918ff41 76
LtBarbershop 1:3a40c918ff41 77 DE0.write(0x8004); // SPI slave Control word to read (only) 4-word transactions starting at base address 0 within the peripheral
LtBarbershop 1:3a40c918ff41 78 PeriodicInt.attach(&PiControllerISR, .02); // Specify address of the TimerISR (Ticker) function and the interval between interrupts
LtBarbershop 1:3a40c918ff41 79 BluetoothSerial.printf("\n\n\rTap w-a-s-d keys for differential speed control: ");
LtBarbershop 1:3a40c918ff41 80 do {
LtBarbershop 1:3a40c918ff41 81
LtBarbershop 1:3a40c918ff41 82 /*if (pc.readable()){
LtBarbershop 1:3a40c918ff41 83 x=pc.getc();
LtBarbershop 1:3a40c918ff41 84 pc.putc(x); //Echo keyboard entry
LtBarbershop 1:3a40c918ff41 85 osTimerStart(OneShot, 2000); // Set the watchdog timer interrupt to 2s.
LtBarbershop 1:3a40c918ff41 86
LtBarbershop 1:3a40c918ff41 87 }*/
LtBarbershop 1:3a40c918ff41 88 if(pc.readable())
LtBarbershop 1:3a40c918ff41 89 {
LtBarbershop 1:3a40c918ff41 90 pc.printf("\n\r Enter a motor speed (with sign as direction:\n\r");
LtBarbershop 1:3a40c918ff41 91 pc.scanf("%f", &u1);
LtBarbershop 1:3a40c918ff41 92 pc.printf("%f", u1);
LtBarbershop 1:3a40c918ff41 93 /* x=pc.getc();
LtBarbershop 1:3a40c918ff41 94 if(x=='w')
LtBarbershop 1:3a40c918ff41 95 {
LtBarbershop 1:3a40c918ff41 96 // increase motor speed
LtBarbershop 1:3a40c918ff41 97 u1 += 0.02;
LtBarbershop 1:3a40c918ff41 98 if (u1 > 1)
LtBarbershop 1:3a40c918ff41 99 {
LtBarbershop 1:3a40c918ff41 100 u1 = 1;
LtBarbershop 1:3a40c918ff41 101 }
LtBarbershop 1:3a40c918ff41 102 }
LtBarbershop 1:3a40c918ff41 103 else if(x=='s')
LtBarbershop 1:3a40c918ff41 104 {
LtBarbershop 1:3a40c918ff41 105 // u1ecrease motor speed
LtBarbershop 1:3a40c918ff41 106 u1 -= 0.02;
LtBarbershop 1:3a40c918ff41 107 if (u1 < 0)
LtBarbershop 1:3a40c918ff41 108 {
LtBarbershop 1:3a40c918ff41 109 u1 = 0;
LtBarbershop 1:3a40c918ff41 110 }
LtBarbershop 1:3a40c918ff41 111 }
LtBarbershop 1:3a40c918ff41 112 //else if(x=='a') ...
LtBarbershop 1:3a40c918ff41 113 //else if(x=='d') ...
LtBarbershop 1:3a40c918ff41 114 */
LtBarbershop 1:3a40c918ff41 115 if (u1 > 1)
LtBarbershop 1:3a40c918ff41 116 {
LtBarbershop 1:3a40c918ff41 117 u1 = 1;
LtBarbershop 1:3a40c918ff41 118 }
LtBarbershop 1:3a40c918ff41 119
LtBarbershop 1:3a40c918ff41 120 if (u1 < -1)
LtBarbershop 1:3a40c918ff41 121 {
LtBarbershop 1:3a40c918ff41 122 u1 = -1;
LtBarbershop 1:3a40c918ff41 123 }
LtBarbershop 1:3a40c918ff41 124 if (u1 > 0)
LtBarbershop 1:3a40c918ff41 125 {
LtBarbershop 1:3a40c918ff41 126 dirL = 1;
LtBarbershop 1:3a40c918ff41 127 }
LtBarbershop 1:3a40c918ff41 128 else
LtBarbershop 1:3a40c918ff41 129 {
LtBarbershop 1:3a40c918ff41 130 dirL = 0;
LtBarbershop 1:3a40c918ff41 131 }
LtBarbershop 1:3a40c918ff41 132 }
LtBarbershop 1:3a40c918ff41 133
LtBarbershop 1:3a40c918ff41 134 // Display variables at the terminal emulator for logging:
LtBarbershop 1:3a40c918ff41 135 //pc.printf("\r\n%6d %6d %6d %6d %6d %6d", ... );
LtBarbershop 1:3a40c918ff41 136 Thread::wait(500); // Wait 500 ms
LtBarbershop 1:3a40c918ff41 137 }
LtBarbershop 1:3a40c918ff41 138 while(1);
LtBarbershop 1:3a40c918ff41 139 }
LtBarbershop 1:3a40c918ff41 140
LtBarbershop 1:3a40c918ff41 141 // ******** Control Thread ********
LtBarbershop 1:3a40c918ff41 142 void PiControlThread(void const *argument) {
LtBarbershop 1:3a40c918ff41 143
LtBarbershop 1:3a40c918ff41 144 while (true) {
LtBarbershop 1:3a40c918ff41 145 osSignalWait(SignalPi, osWaitForever);
LtBarbershop 1:3a40c918ff41 146 led2= !led2; // Alive status
LtBarbershop 1:3a40c918ff41 147
LtBarbershop 1:3a40c918ff41 148 float T;
LtBarbershop 1:3a40c918ff41 149 float d;
LtBarbershop 1:3a40c918ff41 150 T = 0.001;
LtBarbershop 1:3a40c918ff41 151 d = abs(u1);
LtBarbershop 1:3a40c918ff41 152 /*if (u1 < 0)
LtBarbershop 1:3a40c918ff41 153 {
LtBarbershop 1:3a40c918ff41 154 dir1 = 1;
LtBarbershop 1:3a40c918ff41 155 }
LtBarbershop 1:3a40c918ff41 156 else
LtBarbershop 1:3a40c918ff41 157 {
LtBarbershop 1:3a40c918ff41 158 dir1 = 0;
LtBarbershop 1:3a40c918ff41 159 }
LtBarbershop 1:3a40c918ff41 160 */
LtBarbershop 1:3a40c918ff41 161 PwmSetOut(d, T);
LtBarbershop 1:3a40c918ff41 162 }
LtBarbershop 1:3a40c918ff41 163 }
LtBarbershop 1:3a40c918ff41 164
LtBarbershop 1:3a40c918ff41 165 // ******** Collision Thread ********
LtBarbershop 1:3a40c918ff41 166 void ExtCollisionThread(void const *argument) {
LtBarbershop 1:3a40c918ff41 167 while (true) {
LtBarbershop 1:3a40c918ff41 168 osSignalWait(SignalExtCollision, osWaitForever);
LtBarbershop 1:3a40c918ff41 169 led4 = !led4;
LtBarbershop 1:3a40c918ff41 170 }
LtBarbershop 1:3a40c918ff41 171 }
LtBarbershop 1:3a40c918ff41 172
LtBarbershop 1:3a40c918ff41 173 // ******** Watchdog Interrupt Handler ********
LtBarbershop 1:3a40c918ff41 174 void Watchdog(void const *n) {
LtBarbershop 1:3a40c918ff41 175 led3=1;
LtBarbershop 1:3a40c918ff41 176 }
LtBarbershop 1:3a40c918ff41 177
LtBarbershop 1:3a40c918ff41 178 // ******** Period Timer Interrupt Handler ********
LtBarbershop 1:3a40c918ff41 179 void PiControllerISR(void) {
LtBarbershop 1:3a40c918ff41 180 osSignalSet(PiControl,0x1);
LtBarbershop 1:3a40c918ff41 181 }
LtBarbershop 1:3a40c918ff41 182
LtBarbershop 1:3a40c918ff41 183 // ******** Collision Interrupt Handler ********
LtBarbershop 1:3a40c918ff41 184 void ExtCollisionISR(void)
LtBarbershop 1:3a40c918ff41 185 {
LtBarbershop 1:3a40c918ff41 186 osSignalSet(ExtCollision,0x1);
LtBarbershop 1:3a40c918ff41 187 }
LtBarbershop 1:3a40c918ff41 188
LtBarbershop 1:3a40c918ff41 189 //
LtBarbershop 1:3a40c918ff41 190 void PwmSetOut(float d, float T)
LtBarbershop 1:3a40c918ff41 191 {
LtBarbershop 1:3a40c918ff41 192 PwmOut PwmP21(p21);
LtBarbershop 1:3a40c918ff41 193 float onTime = d * T;
LtBarbershop 1:3a40c918ff41 194
LtBarbershop 1:3a40c918ff41 195 PwmP21.period(T);
LtBarbershop 1:3a40c918ff41 196 PwmP21.pulsewidth(onTime);
LtBarbershop 1:3a40c918ff41 197 }
LtBarbershop 1:3a40c918ff41 198
LtBarbershop 1:3a40c918ff41 199 void ReadEncoder()
LtBarbershop 1:3a40c918ff41 200 {
LtBarbershop 1:3a40c918ff41 201 int dPositionRight, dTimeRight, dPositionLeft, dTimeLeft;
LtBarbershop 1:3a40c918ff41 202
LtBarbershop 1:3a40c918ff41 203 // May be executed in a loop
LtBarbershop 1:3a40c918ff41 204 dPositionRight = DE0.write(Dummy); // Read QEI-0 position register
LtBarbershop 1:3a40c918ff41 205 dTimeRight = DE0.write(Dummy); // Read QE-0 time interval register
LtBarbershop 1:3a40c918ff41 206 dPositionLeft = DE0.write(Dummy); // Read QEI-1 position register
LtBarbershop 1:3a40c918ff41 207 dTimeLeft = DE0.write(Dummy); // Read QEI-1 time interval register
LtBarbershop 1:3a40c918ff41 208 }
LtBarbershop 1:3a40c918ff41 209
LtBarbershop 1:3a40c918ff41 210 void InitializeEncoder()
LtBarbershop 1:3a40c918ff41 211 {
LtBarbershop 1:3a40c918ff41 212 // Initialization – to be executed once (normally)
LtBarbershop 1:3a40c918ff41 213 DE0.format(16,0); // SPI format: 16-bit words, mode 0 protocol.
LtBarbershop 1:3a40c918ff41 214 SpiStart = 0;
LtBarbershop 1:3a40c918ff41 215 SpiReset = 1;
LtBarbershop 1:3a40c918ff41 216 wait_us(10);
LtBarbershop 1:3a40c918ff41 217 SpiReset = 0;
LtBarbershop 1:3a40c918ff41 218 DE0.write(0x8004); // SPI slave control word to read (only) 4-word transactions
LtBarbershop 1:3a40c918ff41 219 // starting at base address 0 within the peripheral.
LtBarbershop 1:3a40c918ff41 220 }
LtBarbershop 0:6321191f814a 221