Diff: main.cpp

Revision:

1:3a40c918ff41

Parent:

0:6321191f814a

Child:

2:1c5cc180812d

--- a/main.cpp	Fri Feb 08 20:55:26 2013 +0000
+++ b/main.cpp	Wed Feb 13 21:08:05 2013 +0000
@@ -1,174 +1,221 @@
-#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);
-}
-
-   
+#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);
+DigitalOut dirL(p22);
+
+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);
+void ReadEncoder();
+void InitializeEncoder();
+
+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: \r\n");    
+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()) 
+    {
+        pc.printf("\n\r Enter a motor speed (with sign as direction:\n\r");
+        pc.scanf("%f", &u1);
+        pc.printf("%f", u1);
+        /* 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') ... 
+        */
+            if (u1 > 1)
+            {
+                u1 = 1;
+            }
+            
+            if (u1 < -1)
+            {
+                u1 = -1;
+            }
+            if (u1 > 0)
+            {
+                dirL = 1;
+            }            
+            else
+            {
+                dirL = 0;
+            }
+    }
+        
+    // 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 T;
+    float d;
+    T = 0.001;
+    d = abs(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);
+}
+
+void ReadEncoder()
+{
+    int dPositionRight, dTimeRight, dPositionLeft, dTimeLeft;
+    
+    // May be executed in a loop
+    dPositionRight = DE0.write(Dummy); // Read QEI-0 position register 
+    dTimeRight = DE0.write(Dummy);     // Read QE-0 time interval register
+    dPositionLeft = DE0.write(Dummy);  // Read QEI-1 position register 
+    dTimeLeft = DE0.write(Dummy);      // Read QEI-1 time interval register
+}
+   
+void InitializeEncoder()
+{
+    // Initialization – to be executed once (normally)
+    DE0.format(16,0);   // SPI format: 16-bit words, mode 0 protocol.
+    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.
+}
     
\ No newline at end of file