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@0:6321191f814a, 2013-02-08 (annotated)

Committer:
LtBarbershop
Date:
Fri Feb 08 20:55:26 2013 +0000
Revision:
0:6321191f814a
Child:
1:3a40c918ff41
first commit

Who changed what in which revision?

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