Diff: main.cpp

Revision:

0:6321191f814a

Child:

1:3a40c918ff41

diff -r 000000000000 -r 6321191f814a main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Feb 08 20:55:26 2013 +0000
@@ -0,0 +1,174 @@
+#include "mbed.h"
+#include "rtos.h"
+
+
+// C.P. Diduch
+// EE4333 Robotics Lab-3 
+// Implementation of a PI Speed Control System
+// December 17, 2012.
+
+#define Dummy 0
+
+
+// Function prototypes
+void PiControllerISR(void);
+void WdtFaultISR(void);
+void ExtCollisionISR(void);
+void PiControlThread(void const *argument);
+void ExtCollisionThread(void const *argument);
+void Watchdog(void const *n);
+
+// Global variables for interrupt handler
+float u1;
+float u2;
+// Processes and threads
+int32_t SignalPi, SignalWdt, SignalExtCollision;
+osThreadId PiControl,WdtFault,ExtCollision;
+osThreadDef(PiControlThread, osPriorityNormal, DEFAULT_STACK_SIZE);
+osThreadDef(ExtCollisionThread, osPriorityNormal, DEFAULT_STACK_SIZE);
+osTimerDef(Wdtimer, Watchdog);
+
+// IO Port Configuration
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+DigitalOut led4(LED4);
+
+Serial BluetoothSerial(p28, p27); // (tx, rx) for PC serial channel
+Serial pc(USBTX, USBRX); // (tx, rx) for Parani/Promi Bluetooth serial channel
+
+// Prototypes
+void PwmSetOut(float d, float T);
+
+
+Ticker PeriodicInt;                 
+SPI DE0(p5, p6, p7); // (mosi, miso, sclk) DE0 is the SPI channel with the DE0 FPGA 
+DigitalOut SpiReset(p11); // Reset for all devices within the slave SPI peripheral in the DE0 FPGA
+DigitalOut SpiStart(p12); // Places SPI interace on the DE0 FPGA into control mode
+DigitalOut dir1(p22);
+
+// ******** Main Thread ********
+int main() {
+char x;
+char string[30];
+
+float d = 0.1;
+float T = 0.001;
+
+led3=0;
+led4=0;  
+
+InterruptIn Bumper(p8);  // External interrupt pin
+Bumper.rise(&ExtCollisionISR); // Atach the address of the interrupt handler to the rising edge of Bumper
+
+// Start execution of the Threads
+PiControl = osThreadCreate(osThread(PiControlThread), NULL);
+ExtCollision = osThreadCreate(osThread(ExtCollisionThread), NULL);
+osTimerId OneShot = osTimerCreate(osTimer(Wdtimer), osTimerOnce, (void *)0);
+
+pc.printf("\r\n RTOS Template");    
+SpiStart=0;
+SpiReset=1;
+wait_us(10);
+SpiReset=0;
+
+DE0.write(0x8004); // SPI slave Control word to read (only) 4-word transactions starting at base address 0 within the peripheral
+PeriodicInt.attach(&PiControllerISR, .02); // Specify address of the TimerISR (Ticker) function and the interval between interrupts
+BluetoothSerial.printf("\n\n\rTap w-a-s-d keys for differential speed control: ");
+do {
+
+    if (pc.readable()){
+        x=pc.getc();
+        pc.putc(x); //Echo keyboard entry
+        osTimerStart(OneShot, 2000); // Set the watchdog timer interrupt to 2s.
+           
+        }
+    if(pc.readable()) {
+        x=pc.getc();
+        if(x=='w')
+        {
+            // increase motor speed
+            u1 += 0.02;
+            if (u1 > 1)
+            {
+                u1 = 1;
+            }
+        }
+        else if(x=='s')
+        {
+            // u1ecrease motor speed
+            u1 -= 0.02;
+            if (u1 < 0)
+            {
+                u1 = 0;
+            }
+        }
+        //else if(x=='a') ...
+        //else if(x=='d') ...
+    }
+        
+    // Display variables at the terminal emulator for logging:
+    //pc.printf("\r\n%6d %6d %6d %6d %6d %6d", ... );
+    Thread::wait(500); // Wait 500 ms
+}
+while(1);
+}
+
+// ******** Control Thread ********
+void PiControlThread(void const *argument) {
+
+while (true) {
+    osSignalWait(SignalPi, osWaitForever); 
+    led2= !led2; // Alive status
+    
+    float d;
+    d = u1;
+    if (u1 < 0)
+    {
+        dir1 = 1;
+    }
+    else
+    {
+        dir1 = 0;
+    }
+    
+    PwmSetOut(d, T);
+   } 
+ }
+
+// ******** Collision Thread ********
+void ExtCollisionThread(void const *argument) {
+while (true) {
+    osSignalWait(SignalExtCollision, osWaitForever);
+    led4 = !led4;
+    }
+}
+
+// ******** Watchdog Interrupt Handler ********
+void Watchdog(void const *n) {
+    led3=1;
+}
+
+// ******** Period Timer Interrupt Handler ********
+void PiControllerISR(void) {
+    osSignalSet(PiControl,0x1);
+    }
+    
+// ******** Collision Interrupt Handler ********
+void ExtCollisionISR(void) 
+{
+    osSignalSet(ExtCollision,0x1);
+}
+
+//
+void PwmSetOut(float d, float T)
+{
+    PwmOut PwmP21(p21);
+    float onTime = d * T; 
+
+    PwmP21.period(T);
+    PwmP21.pulsewidth(onTime);
+}
+
+   
+    
\ No newline at end of file