main.cpp

00001 #include "mbed.h"
00002 #include "rtos.h"
00003 
00004 
00005 // C.P. Diduch
00006 // EE4333 Robotics Lab-3 
00007 // Implementation of a PI Speed Control System
00008 // December 17, 2012.
00009 
00010 #define Dummy 0
00011 
00012 
00013 // Function prototypes
00014 void PiControllerISR(void);
00015 void WdtFaultISR(void);
00016 void ExtCollisionISR(void);
00017 void PiControlThread(void const *argument);
00018 void ExtCollisionThread(void const *argument);
00019 void Watchdog(void const *n);
00020 
00021 // Global variables for interrupt handler
00022 float u1;
00023 float u2;
00024 // Processes and threads
00025 int32_t SignalPi, SignalWdt, SignalExtCollision;
00026 osThreadId PiControl,WdtFault,ExtCollision;
00027 osThreadDef(PiControlThread, osPriorityNormal, DEFAULT_STACK_SIZE);
00028 osThreadDef(ExtCollisionThread, osPriorityNormal, DEFAULT_STACK_SIZE);
00029 osTimerDef(Wdtimer, Watchdog);
00030 
00031 // IO Port Configuration
00032 DigitalOut led1(LED1);
00033 DigitalOut led2(LED2);
00034 DigitalOut led3(LED3);
00035 DigitalOut led4(LED4);
00036 DigitalOut dirL(p22);
00037 
00038 Serial BluetoothSerial(p28, p27); // (tx, rx) for PC serial channel
00039 Serial pc(USBTX, USBRX); // (tx, rx) for Parani/Promi Bluetooth serial channel
00040 
00041 // Prototypes
00042 void PwmSetOut(float d, float T);
00043 void ReadEncoder();
00044 void InitializeEncoder();
00045 
00046 Ticker PeriodicInt;                 
00047 SPI DE0(p5, p6, p7); // (mosi, miso, sclk) DE0 is the SPI channel with the DE0 FPGA 
00048 DigitalOut SpiReset(p11); // Reset for all devices within the slave SPI peripheral in the DE0 FPGA
00049 DigitalOut SpiStart(p12); // Places SPI interace on the DE0 FPGA into control mode
00050 DigitalOut dir1(p22);
00051 
00052 // ******** Main Thread ********
00053 int main() {
00054 char x;
00055 char string[30];
00056 
00057 //float d = 0.1;
00058 //float T = 0.001;
00059 
00060 led3=0;
00061 led4=0;  
00062 
00063 InterruptIn Bumper(p8);  // External interrupt pin
00064 Bumper.rise(&ExtCollisionISR); // Atach the address of the interrupt handler to the rising edge of Bumper
00065 
00066 // Start execution of the Threads
00067 PiControl = osThreadCreate(osThread(PiControlThread), NULL);
00068 ExtCollision = osThreadCreate(osThread(ExtCollisionThread), NULL);
00069 osTimerId OneShot = osTimerCreate(osTimer(Wdtimer), osTimerOnce, (void *)0);
00070 
00071 pc.printf("\r\n RTOS Template: \r\n");    
00072 SpiStart=0;
00073 SpiReset=1;
00074 wait_us(10);
00075 SpiReset=0;
00076 
00077 DE0.write(0x8004); // SPI slave Control word to read (only) 4-word transactions starting at base address 0 within the peripheral
00078 PeriodicInt.attach(&PiControllerISR, .02); // Specify address of the TimerISR (Ticker) function and the interval between interrupts
00079 BluetoothSerial.printf("\n\n\rTap w-a-s-d keys for differential speed control: ");
00080 do {
00081 
00082     /*if (pc.readable()){
00083         x=pc.getc();
00084         pc.putc(x); //Echo keyboard entry
00085         osTimerStart(OneShot, 2000); // Set the watchdog timer interrupt to 2s.
00086            
00087         }*/
00088     if(pc.readable()) 
00089     {
00090         pc.printf("\n\r Enter a motor speed (with sign as direction:\n\r");
00091         pc.scanf("%f", &u1);
00092         pc.printf("%f", u1);
00093         /* x=pc.getc();
00094         if(x=='w')
00095         {
00096             // increase motor speed
00097             u1 += 0.02;
00098             if (u1 > 1)
00099             {
00100                 u1 = 1;
00101             }
00102         }
00103         else if(x=='s')
00104         {
00105             // u1ecrease motor speed
00106             u1 -= 0.02;
00107             if (u1 < 0)
00108             {
00109                 u1 = 0;
00110             }
00111         }
00112         //else if(x=='a') ...
00113         //else if(x=='d') ... 
00114         */
00115             if (u1 > 1)
00116             {
00117                 u1 = 1;
00118             }
00119             
00120             if (u1 < -1)
00121             {
00122                 u1 = -1;
00123             }
00124             if (u1 > 0)
00125             {
00126                 dirL = 1;
00127             }            
00128             else
00129             {
00130                 dirL = 0;
00131             }
00132     }
00133         
00134     // Display variables at the terminal emulator for logging:
00135     //pc.printf("\r\n%6d %6d %6d %6d %6d %6d", ... );
00136     Thread::wait(500); // Wait 500 ms
00137 }
00138 while(1);
00139 }
00140 
00141 // ******** Control Thread ********
00142 void PiControlThread(void const *argument) {
00143 
00144 while (true) {
00145     osSignalWait(SignalPi, osWaitForever); 
00146     led2= !led2; // Alive status
00147     
00148     float T;
00149     float d;
00150     T = 0.001;
00151     d = abs(u1);
00152     /*if (u1 < 0)
00153     {
00154         dir1 = 1;
00155     }
00156     else
00157     {
00158         dir1 = 0;
00159     }
00160     */
00161     PwmSetOut(d, T);
00162    } 
00163  }
00164 
00165 // ******** Collision Thread ********
00166 void ExtCollisionThread(void const *argument) {
00167 while (true) {
00168     osSignalWait(SignalExtCollision, osWaitForever);
00169     led4 = !led4;
00170     }
00171 }
00172 
00173 // ******** Watchdog Interrupt Handler ********
00174 void Watchdog(void const *n) {
00175     led3=1;
00176 }
00177 
00178 // ******** Period Timer Interrupt Handler ********
00179 void PiControllerISR(void) {
00180     osSignalSet(PiControl,0x1);
00181     }
00182     
00183 // ******** Collision Interrupt Handler ********
00184 void ExtCollisionISR(void) 
00185 {
00186     osSignalSet(ExtCollision,0x1);
00187 }
00188 
00189 //
00190 void PwmSetOut(float d, float T)
00191 {
00192     PwmOut PwmP21(p21);
00193     float onTime = d * T; 
00194 
00195     PwmP21.period(T);
00196     PwmP21.pulsewidth(onTime);
00197 }
00198 
00199 void ReadEncoder()
00200 {
00201     int dPositionRight, dTimeRight, dPositionLeft, dTimeLeft;
00202     
00203     // May be executed in a loop
00204     dPositionRight = DE0.write(Dummy); // Read QEI-0 position register 
00205     dTimeRight = DE0.write(Dummy);     // Read QE-0 time interval register
00206     dPositionLeft = DE0.write(Dummy);  // Read QEI-1 position register 
00207     dTimeLeft = DE0.write(Dummy);      // Read QEI-1 time interval register
00208 }
00209    
00210 void InitializeEncoder()
00211 {
00212     // Initialization – to be executed once (normally)
00213     DE0.format(16,0);   // SPI format: 16-bit words, mode 0 protocol.
00214     SpiStart = 0;
00215     SpiReset = 1;
00216     wait_us(10);
00217     SpiReset = 0;
00218     DE0.write(0x8004);  // SPI slave control word to read (only) 4-word transactions
00219                             // starting at base address 0 within the peripheral.
00220 }
00221