Diff: main.cpp

Revision:

2:1c5cc180812d

Parent:

1:3a40c918ff41

Child:

3:9a39e487b724

--- a/main.cpp	Wed Feb 13 21:08:05 2013 +0000
+++ b/main.cpp	Fri Feb 15 18:46:11 2013 +0000
@@ -1,184 +1,177 @@
+// Robot Control Code
+// Tom Elliott and Ian Colwell
+
+
 #include "mbed.h"
 #include "rtos.h"
 
+// --- Constants
+#define Dummy 0
+#define PWMPeriod 0.001
 
-// C.P. Diduch
-// EE4333 Robotics Lab-3 
-// Implementation of a PI Speed Control System
-// December 17, 2012.
-
-#define Dummy 0
-
-
-// Function prototypes
+// --- 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);
+void InitializeSystem();
+void InitializeEncoder();
+void InitializePWM();
+void PwmSetOut(float d, float T);
+void ReadEncoder();
+void SetLeftMotorSpeed(float u);
+void SetRightMotorSpeed(float u);
 
 // Global variables for interrupt handler
 float u1;
 float u2;
-// Processes and threads
+
+// --- 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
+/* PIN-OUT
+
+MOSI Quad Enc    5|-|
+MISO Quad Enc    6|-|
+SCK Quad Enc     7|-|
+SPI Start Quad E 8|-|
+SPI Reset Quad E 9|-|
+
+Bluetooth tx    13|-|28
+Bluetooth rx    14|-|27
+                15|-|26 Brake, Right Motor, M2
+                16|-|25 Dir, Right Motor, M2
+                17|-|24 PWM, Right Motor, M2
+                18|-|23 Brake, Left Motor, M1
+                19|-|22 Dir, Left Motor, M1
+                20|-|21 PWM, Left Motor, M1
+*/
+
+// --- 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
+DigitalOut brakeL(p23);
+PwmOut PwmL(p21);
+DigitalOut dirR(p25);
+DigitalOut brakeR(p26);
+PwmOut PwmR(p24);
+Serial BluetoothSerial(p13, p14); // (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();
+SPI DE0(p5, p6, p7); // (mosi, miso, sclk) DE0 is the SPI channel with the DE0 FPGA 
+DigitalOut SpiReset(p9); // Reset for all devices within the slave SPI peripheral in the DE0 FPGA
+DigitalOut SpiStart(p8); // Places SPI interace on the DE0 FPGA into control mode
+InterruptIn Bumper(p10);  // External interrupt pin
 
 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);
+int main() 
+{    
+    InitializeSystem();
+    
+    pc.printf("\r\n Robot Initialization Complete \r\n");    
+    
+    BluetoothSerial.printf("\n\n\rTap w-a-s-d keys for differential speed control: ");
+    
+    while(1)
+    {
 
-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')
+        /*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()) 
         {
-            // increase motor speed
-            u1 += 0.02;
-            if (u1 > 1)
+            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')
             {
-                u1 = 1;
+                // 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)
+            else if(x=='s')
             {
-                u1 = 1;
-            }
-            
-            if (u1 < -1)
-            {
-                u1 = -1;
+                // u1ecrease motor speed
+                u1 -= 0.02;
+                if (u1 < 0)
+                {
+                    u1 = 0;
+                }
             }
-            if (u1 > 0)
-            {
-                dirL = 1;
-            }            
-            else
-            {
-                dirL = 0;
-            }
+            //else if(x=='a') ...
+            //else if(x=='d') ... 
+            */
+                if (u1 > 1)
+                {
+                    u1 = 1;
+                }
+                
+                if (u1 < -1)
+                {
+                    u1 = -1;
+                }
+        }
+            
+        Thread::wait(500); // Wait 500 ms
     }
-        
-    // 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) {
+void PiControlThread(void const *argument) 
+{
+    while (1) 
+    {
+        osSignalWait(SignalPi, osWaitForever); 
+        led2= !led2; // Alive status
+        
+        // Read encoder and display results
+        ReadEncoder();
+        
+        SetLeftMotorSpeed(u1);
+        SetRightMotorSpeed(u2);
 
-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;
+void ExtCollisionThread(void const *argument) 
+{
+    while (1) 
+    {
+        osSignalWait(SignalExtCollision, osWaitForever);
+        led4 = !led4;
     }
 }
 
 // ******** Watchdog Interrupt Handler ********
-void Watchdog(void const *n) {
+void Watchdog(void const *n) 
+{
     led3=1;
 }
 
 // ******** Period Timer Interrupt Handler ********
-void PiControllerISR(void) {
+void PiControllerISR(void) 
+{
     osSignalSet(PiControl,0x1);
-    }
+}
     
 // ******** Collision Interrupt Handler ********
 void ExtCollisionISR(void) 
@@ -186,14 +179,86 @@
     osSignalSet(ExtCollision,0x1);
 }
 
-//
-void PwmSetOut(float d, float T)
+// --- Initialization Functions
+void InitializeSystem()
+{
+    led3=0;
+    led4=0; 
+    
+    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);
+    PeriodicInt.attach(&PiControllerISR, .02); // Specify address of the TimerISR (Ticker) function and the interval between interrupts
+    
+    InitializeEncoder();
+}
+
+void InitializePWM()
+{
+
+}
+
+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.
+}
+
+
+// --- Other Functions
+void SetLeftMotorSpeed(float u)
 {
-    PwmOut PwmP21(p21);
-    float onTime = d * T; 
+    float T;
+    float d;
+    float onTime;
+    
+    T = PWMPeriod;
+    d = abs(u);
+    onTime = d * T; 
+
+    PwmL.period(T);
+    PwmL.pulsewidth(onTime);
+    
+    if (u > 0)
+    {
+        dirL = 1;
+    }            
+    else
+    {
+        dirL = 0;
+    }
+}
 
-    PwmP21.period(T);
-    PwmP21.pulsewidth(onTime);
+void SetRightMotorSpeed(float u)
+{
+    float T;
+    float d;
+    float onTime;
+    
+    T = PWMPeriod;
+    d = abs(u);
+    onTime = d * T; 
+
+    PwmR.period(T);
+    PwmR.pulsewidth(onTime);
+    
+    if (u > 0)
+    {
+        dirR = 1;
+    }            
+    else
+    {
+        dirR = 0;
+    }
 }
 
 void ReadEncoder()
@@ -205,17 +270,10 @@
     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
+    
+    // simply write out the results for now
+    pc.printf("Left Position: %d \n\r", dPositionLeft);
+    pc.printf("Left Time: %d \n\r", dTimeLeft);
+    pc.printf("Right Position: %d \n\r", dPositionRight);
+    pc.printf("Right Time: %d \n\n\r", dTimeRight);
+}  
\ No newline at end of file