Diff: main.cpp

Revision:

0:867c4eabf128

Child:

1:f1d9b55f7dd7

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Fri Feb 26 21:28:57 2016 +0000
@@ -0,0 +1,359 @@
+#include "mbed.h"
+#include "rtos.h"
+#include "stdlib.h"
+
+// IO Port Configuration
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+DigitalOut led4(LED4);
+PwmOut PwmR(p21); // define p21 as PwmR, p22 as PwmL
+PwmOut PwmL(p22); // define p21 as PwmR, p22 as PwmL
+DigitalOut DirR(p30); // define p30 as DirR, p29 as DirL
+DigitalOut DirL(p29); // define p30 as DirR, p29 as DirL
+DigitalOut SpiReset(p11);//Reset for all devices within the slave SPI peripheral in the DE0 FPGA
+DigitalOut IoReset(p12); //Places SPI interface on the DE0 FPGA into control mode
+SPI DE0(p5,p6,p7); //(mosi,miso,sclk)DE0 is the SPI channel with the FPGA
+Serial pc(USBTX, USBRX); // Pins (tx, rx) for PC serial channel
+Serial BluetoothSerial(p9, p10); // tx, rx
+Ticker PeriodicInt;      // Declare a timer interrupt: PeriodicInt
+InterruptIn Bumper(p8);  // External interrupt pin declared as Bumper
+
+// Function prototypes
+void Initialize(void);
+char DisplayMenu(void);
+void ManualControl(void);
+void BluetoothTest(void);
+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 PIController(void);
+void SpeedControl(void);
+int SatAdd(int x, int y);
+int SatSub(int x, int y);
+int SatValue(int x, int Limit);
+int Signextend(int x);
+void Userinterface(void);
+void QeiInterface(void);
+void UsRangefinder(void);
+void RcServoMotor(void);
+
+
+// Processes and threads
+int32_t SignalPi, SignalWdt, SignalExtCollision;
+osThreadId PiControl,WdtFault,ExtCollision;
+osThreadDef(PiControlThread, osPriorityRealtime, DEFAULT_STACK_SIZE); // Declare Control as a thread/process
+osThreadDef(ExtCollisionThread, osPriorityHigh, DEFAULT_STACK_SIZE);  // Declare External Collision as a thread/process
+osTimerDef(Wdtimer, Watchdog); // Declare a watch dog timer
+
+
+// Global variables for interrupt handler
+int32_t dPos_R, dTime_R, Vel_R;
+int32_t Setspeed_R;
+int32_t u_R=0,uS_R=0;
+int32_t uP_R,uI_R;
+int32_t e_R;
+int32_t x_R=0, xT_R=0;
+
+float T = 5000;  //us
+float y_R=0;
+float Kp_R=3.5, Ki_R=0.8;  
+
+
+//**************************************************************************
+//**************************************************************************
+
+
+int main()
+{
+
+    //Initialize();
+
+    //Userinterface();
+    
+    //UsRangefinder();
+    
+    RcServoMotor();
+
+}
+
+
+void UsRangefinder(void)
+{
+    unsigned int V, duration;
+    float distance;
+    DE0.format(16,1); // SPI is mode 1
+    DE0.frequency(500000); // SPI frequency
+
+    IoReset=0; // Reset all SPI peripherals
+    IoReset=1;
+    wait_us(5);
+    IoReset=0;
+
+    SpiReset=0;
+    SpiReset=1;
+    wait_us(5);
+    SpiReset=0;
+    V=DE0.write(0x8901); // Control word to read ultrasonic range finder.
+    do {
+        duration = DE0.write(0x0000); // Read ultrasonic rangefinder.
+        pc.printf("\r\n SPI ID=%4x Us Range=%5d",V,duration);
+        
+        //Need to check if duration is singed or unsigned value, if extend 16bit to 32bit
+        distance = (duration / 2) * 0.0344;
+        
+        if (distance >= 400 || distance <= 2){
+            pc.printf("\r\nOut of Range\r\n");
+            }
+        else {
+            pc.printf("Distance = %2f", distance);
+            wait(1);
+            }
+        wait(0.5);
+    } while(!pc.readable());
+}
+
+void RcServoMotor(void)
+{
+    unsigned int V, W, X, n;
+    DE0.format(16,1); // SPI is mode 1
+    DE0.frequency(500000); // SPI frequency
+    IoReset=0; // Reset all SPI peripherals
+    IoReset=1;
+    wait_us(5);
+    IoReset=0;
+
+    SpiReset=0;
+    SpiReset=1;
+    wait_us(5);
+    SpiReset=0;
+    V=DE0.write(0x0402); // Control word to write RC Servo pulse interval in ms.
+    //n=4000;
+    
+    do {
+        /*
+        wait_us(10);
+        W=DE0.write(0x10); // Write RC Servo pulse interval in ms
+        X=DE0.write(n); // Write position to RC Servo M1.
+        n=n+2000;
+        if(n==60000) n=4000;
+        pc.printf("\r\n SPI ID=%4x Nms=%4x R/C Servo 1=%5d",V,W,X);
+        wait(1);
+    
+        */
+    
+        for(n=0;n<=60000;n=n+6000){
+            wait_us(10);
+            W=DE0.write(0x10); // Write RC Servo pulse interval in ms
+            X=DE0.write(n); // Write position to RC Servo M1.
+            pc.printf("\r\n SPI ID=%4x Nms=%4x R/C Servo 1=%5d",V,W,X);
+            wait(1);
+        }
+    
+        for(n=60000;n>=6000;n=n-6000){
+            wait_us(10);
+            W=DE0.write(0x10); // Write RC Servo pulse interval in ms
+            X=DE0.write(n); // Write position to RC Servo M1.
+            pc.printf("\r\n SPI ID=%4x Nms=%4x R/C Servo 1=%5d",V,W,X);
+            wait(1);
+    } 
+    
+    } while(!pc.readable());
+    
+}
+
+
+//**************************************************************************
+void Userinterface(void)
+{
+
+    pc.printf("\n\rEnter number:\n ");
+    pc.scanf("%d",&Setspeed_R);
+    pc.printf("\n\rYou Entered :%d\n\n",Setspeed_R);
+
+    /*
+    DirR=1;
+    y_R=Setspeed_R/36*T;
+    pc.printf("y_R = %f\n",y_R);
+    PwmR.pulsewidth_us(y_R);
+    */
+
+}
+
+//**************************************************************************
+void Initialize(void)
+{
+    //Baud Rate
+    //pc.baud (460800); 
+
+    //Intialization Motor
+    PwmR.period_us(T);
+    PwmL.period_us(T);
+
+
+    //Initialization QEI - to be exected once(normally)
+    DE0.format(16,1); //SPI format:16-bit words, mode 1 protocol or mode 0????????????
+    //DE0.frequency(500000);
+    IoReset=0;
+    IoReset=1;
+    IoReset=0;
+
+    SpiReset=0;
+    SpiReset=1;
+    wait_us(10);
+    SpiReset=0;
+    int ID = DE0.write(0x8002);  //SPI slave control word to read (only) 2-word transactions starting at base address 0 within the peripheral
+
+    if(ID == 23) printf("\r\n\nSPI Interface is Successful !  ID = %d\r\n\n",ID); //ID=0x0017
+    else printf("\r\n********SPI Interface failed!********\r\n\n");
+
+
+}
+
+
+//**************************************************************************
+void QeiInterface(void)
+{
+
+    dPos_R = DE0.write(0x00);
+    dTime_R = DE0.write(0x00);
+
+    dPos_R = Signextend(dPos_R);
+   
+
+    Vel_R = (123*dPos_R)/dTime_R;  //radians per second
+
+    printf("\r\ndPos_R: %d dTime_R: %d Vel_R: %d \r\n",dPos_R,dTime_R,Vel_R);
+
+
+}
+
+//**************************************************************************
+
+void PIController(void){
+
+    dPos_R = DE0.write(0x00);
+    dTime_R = DE0.write(0x00);
+
+    dPos_R = Signextend(dPos_R);
+    
+
+    Vel_R = (123*dPos_R)/dTime_R; 
+    
+
+    //Error
+    e_R = SatSub(Setspeed_R,Vel_R);
+    //e_R = SatValue(e_R,36);
+
+
+    //xtate
+    xT_R = SatAdd(x_R,e_R);
+    //xT_R = SatValue(x_R,xlimit);
+
+
+    //proportional and integral control terms
+    uP_R=(Kp_R*e_R);
+    uI_R=(Ki_R*x_R);
+    
+    uS_R = SatAdd(uP_R,uI_R);
+    u_R = SatValue(uS_R,36);
+
+    if (u_R==uS_R) x_R = xT_R;  //Integrator windup prevention
+
+    if(u_R >= 0) DirR = 1; else DirR = 0;
+    
+    y_R=u_R/36.0*T;
+    PwmR.pulsewidth_us(abs(y_R));
+    
+
+}
+
+
+//**************************************************************************
+
+int SatAdd(int x, int y)
+{
+    // Detect overflow and saturate result
+    int z;
+    z = x + y;
+    if((x > 0) && (y > 0) && (z < 0)) z = 0x7FFFFFFF;
+    else if ((x < 0) && (y < 0) && (z > 0)) z = 0x80000000;
+    return z;
+}
+
+
+int SatSub(int x, int y)
+{
+    int z;
+    z = x - y;
+    // 32-bit overflow detection and saturating arithmetic
+    if((x > 0) && (y < 0) && (z < 0)) z = 0x7FFFFFFF;
+    else if((x < 0) && (y > 0) && (z > 0)) z = 0x80000000;
+    return z;
+}
+
+
+int SatValue(int x, int Limit)
+{
+    // Impose maximum limit on integrator state
+    if(x > Limit) return(Limit);
+    else if(x < -Limit) return(-Limit);
+    else return(x);
+}
+
+
+int Signextend(int x) {
+    if (x & 0x00008000) {
+        x = x | 0xFFFF0000;
+    }
+    return x;
+}
+
+//**************************************************************************
+//**************************************************************************
+
+
+// ******** Control Thread ********
+void PiControlThread(void const *argument)
+{
+    while (true) {
+        osSignalWait(SignalPi, osWaitForever); // Go to sleep until signal, SignalPi, is received.
+        led2= !led2; // Alive status - led2 toggles each time PiControlThread is signaled.
+
+        PIController();
+
+    }
+}
+
+// ******** Collision Thread ********
+void ExtCollisionThread(void const *argument)
+{
+    while (true) {
+        osSignalWait(SignalExtCollision, osWaitForever); // Go to sleep until signal, SignalExtCollision, is received
+        led4 = 1;
+    }
+}
+
+// ******** Watchdog Interrupt Handler ********
+void Watchdog(void const *n)
+{
+    led3=1; // led3 is activated when the watchdog timer times out
+}
+
+// ******** Period Timer Interrupt Handler ********
+void PiControllerISR(void)
+{
+    osSignalSet(PiControl,0x1); // Activate the signal, PiControl, with each periodic timer interrupt.
+
+}
+
+// ******** Collision Interrupt Handler ********
+void ExtCollisionISR(void)
+{
+    osSignalSet(ExtCollision,0x1); // Activate the signal, ExtCollision, with each pexternal interrupt.
+}
+
+//**************************************************************************