Diff: Lab3.cpp

Revision:

2:82e4eac97f0a

Parent:

0:b3cd4463d972

Child:

3:30244b9e5351

Child:

4:43aef502bb73

--- a/Lab3.cpp	Sat Feb 13 23:23:08 2016 +0000
+++ b/Lab3.cpp	Fri Feb 19 21:40:48 2016 +0000
@@ -10,59 +10,36 @@
 
 // Function Declarations
 
-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 UserInput(void);
-void DE0_Init(int);
-void L_MotorInit(void);
-void R_MotorInit(void);
+    void DE0_Init(int);
+    void L_MotorInit(void);
+    void R_MotorInit(void);
+    signed int UserInput(void);
+    void ControlThread(void);
+    int SaturateAdd(int x, int y);
+    float SaturateLimit(float x, float limit);
+    signed int SignExtend(signed int x);
 
 // ********************************************************************
 //                     GLOBAL VARIABLE DECLARATIONS
 // ********************************************************************
 
     signed int setpoint;                // Desired Angular Speed ( rad/sec )
-    signed int e;                       // Velocity Error
-    signed int u;                       // Control Signal
-    signed int integrator;              // Integrator State
-
+    float e;                            // Velocity Error
+    float u;                            // Control Signal
+    int L_integrator;                   // Left Integrator State
+    signed int dPositionLeft;           // DE0 Register 0
+    int dTimeLeft;                      // DE0 Register 1
+    
 // *********************************************************************
 //                     PROCESSES AND THREADS
 // *********************************************************************
 
-// Processes and threads
-//int32_t SignalPi, SignalWdt, SignalExtCollision; //Semaphores
-/*
-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
-//typedef enum{ */
-    
-    // Interpretation based on rtos.h 
-    
-    // osPriorityIdle = -3,                 //< priority: idle (lowest)
-    // osPriorityLow = -2,                  //< priority: low
-    // osPriorityBelowNormal = -1,          //< priority: below normal
-    // osPriorityNormal = 0,                //< priority: normal (default)
-    // osPriorityAboveNormal = +1,          //< priority: above normal
-    // osPriorityHigh = +2,                 //< priority: high
-    // osPriorityRealtime = +3,             //< priority: realtime (highest)
-    // osPriority;
-
-
-
 
 // *********************************************************************
 //                          PIN DECLARATIONS
 // *********************************************************************
 
-// Digital I/O Pins
+    // Digital I/O Pins
 
     DigitalOut led1(LED1);              // Thread Indicators
     DigitalOut led2(LED2);              //
@@ -77,68 +54,24 @@
     DigitalOut SpiReset(p11);
     DigitalOut IoReset(p12);
 
-//PWM
+    //PWM
 
     PwmOut PwmL(p22);
     PwmOut PwmR(p21);
 
-//Serial
+    //Serial
 
     Serial pc(USBTX, USBRX);            // tx and rx for PC serial channel via USB cable
     Serial Bluetooth(p9,p10);     // Pins tx(p9) , rx(p10) for bluetooth serial channel
 
-
-//SPI
+    //SPI
 
     SPI DE0(p5,p6,p7); //Pin 5 is MOSI, Pin 6 MISO, Pin 7 SCLK
-
-    
-//Interrupts
-
-    //InterruptIn Bumper(p8);           // External interrupt pin declared as Bumper
+  
+    //Interrupts
+  
     Ticker ControlInterrupt;            // Internal Interrupt to trigger Control Thread
 
-// =====================================================================
-// =====================================================================
-
-
-// *********************************************************************
-//                        MAIN FUNCTION
-// *********************************************************************
-
-int main(){
-   
-   // Initialization
-   
-   DE0_Init(0x8002);                            // Initializes DEO to Mode 1, Varifies Correct Peripheral ID
-                                                // Takes in control word to configure SPI settings                                                                         
-   L_MotorInit();                               // Enables Pwm and sets direction bits for left motor
-   R_MotorInit();                               // Enables Pwm and sets direction bits for right motor
-   integrator = 0;                              // Initializes integrator state to zero ( Global Variable )
-   
-   // Read Inputs from Console
-  
-    UserInput();
-   
-   // Display Global Variables to Console
-   
-   while(1){
-       
-       printf("\n\r ****************************************************");
-       printf("\n\r User Setpoint    : %i",setpoint);      // User defined setpoint
-       printf("\n\r Velocity Error   : %i",e);             // Displays signed Velocity Error
-       printf("\n\r Integrator State : %i",integrator);    // Displays currently state of integrator
-       printf("\n\r Control Signal   : %i",u);             // Displays control signal
-       printf("\n\r ****************************************************");
-       printf("\n\r\n\r");
-       wait(1);
-       
-       // Echo to bluetooth later
-       
-   }
-   
-}
-
 
 // ***************************************************
 //                      DE0 Init
@@ -191,7 +124,7 @@
     // 1 : Forwards  ( Advance )
 
     PwmL.period_us(100);
-    PwmL.pulsewidth_us(0);
+    PwmL.write(0.7);
     
 }
     
@@ -207,11 +140,11 @@
    DirR = 1;                                   // Defaults to 0.
     
     // Direction bit logic output
-    // 0 : Backwards ( Reverse ) 
-    // 1 : Forwards  ( Advance )
+    // 0 : Forwards  ( Advance ) 
+    // 1 : Backwards ( Reverse )
     
     PwmR.period_us(100);
-    PwmR.pulsewidth_us(0);
+    PwmR.write(0);
     
 }
     
@@ -219,61 +152,137 @@
 //                 User Input
 // ***************************************************
 
-void UserInput(){
+signed int UserInput(void){
    
-   Bluetooth.printf("\n\r Please enter a desired angular speed (rad/sec) >> ");
-   printf("\n\r Please enter a desired angular speed (rad/sec) >> ");
+   signed int input;
    
-   if(Bluetooth.readable()){     
-       Bluetooth.scanf("%i",&setpoint);
-       printf("\n\r << Input received via bluetooth >>");}
-   else{
-       scanf("%i",&setpoint);
-       Bluetooth.printf("\n\r << Input received via pc console >>");}
-       
-   printf("\n\r Your number was >> %i\n\r",setpoint);
-   Bluetooth.printf("\n\r Your number was >> %i\n\r",setpoint);
+   printf("\n\r Please enter a desired angular speed (rad/sec) >> ");
+   scanf("%i",&input);
+   printf("\n\r Your number was >> %i\n\r",input);
    
-   wait_ms(500);
-   
-   // Update later to accept period of Pwm ( maybe )
+   return input;
 
 }
 
-/*
 
-// ******** Control Thread ********
+/// ***************************************************
+//                 Control Thread
+// ***************************************************
+
+void ControlThread(void){
+
+    // Read Incremental Position from DE0 QEI
 
-void PiControlThread(void const *argument)
-{
-    while (true) {
-        osSignalWait(SignalPi, osWaitForever); // Go to sleep until, SignalPi, is received.
-        led2 = !led2; // Alive status - led2 toggles each time PiControlThread is signaled.
-        printf("\n\r PI Control Working");
-        if(u >= 0) {
-            if(u>T) {
-                printf("\n\r Maximum Exceeded. Limiting to 1.0 Duty Cycle");
-                u = T;   //Overflow protection
-            }
-            //PW1.pulsewidth_us(u);
-            DIR1 = 1;
-            printf("\n\r Direction: CW");
-        } else if(u < 0) {
-            if(u<-T) {
-                printf("\n\r Maximum Exceeded. Limiting to 1.0 Duty Cycle");
-                u = T;   //Overflow protection
-            }
-            //PW1.pulsewidth_us(u);
-            DIR1 = 0;
-            u = -u;
-            printf("\n\r Selected Pulse Width = %d us", u);
-            printf("\n\r Direction: CCW");
-        }
-        PW1.pulsewidth_us(u);
-        Position = Position + 1;
-        printf("\n\r Duty Cycle = %0.6f", PW1.read());
-        osSignalSet(PiControl, 0x000);
+    int dummy = 0x0000;                                // Pushes dummy information which DE0 ignores, store return from QEI register
+    
+    dPositionLeft = SignExtend(DE0.write(dummy));
+    dTimeLeft = DE0.write(dummy);
+    
+    // Computer Angular Speed and Angular Speed Error
+    
+    signed int AngularSpeedLeft = (123*dPositionLeft)/dTimeLeft;
+    
+    e = setpoint - AngularSpeedLeft;
+    
+    float Kp = 2.5;
+    float Ki = 0.010;
+    
+    if(SaturateLimit((Kp*e+Ki*L_integrator)/35,1)<1){
+        L_integrator = L_integrator +e;}
+    else{
+        L_integrator = L_integrator;
     }
+    
+    
+    u = SaturateLimit( (Kp*e+Ki*L_integrator),1);
+
+    PwmL.write(u);
 }
 
-*/
+/// ***************************************************
+//                SaturateAdd
+// ***************************************************
+
+signed int SaturateAdd(signed int x, signed int y){
+    
+    signed int z = x + y;
+    
+    if( (x>0) && (y>0)&& (z<=0) ){
+        z = 0x7FFFFFFF;}
+    
+    else if( (x<0)&&(y<0)&&(z>=0) ){
+        z = 0x80000000;}
+        
+    return z;
+}
+    
+/// ***************************************************
+//              SaturateLimit
+// ***************************************************
+
+float SaturateLimit(float x, float limit){
+    
+    if (x > limit){
+        return limit;}
+        
+    else if(x < -limit){
+        return(-limit);}
+        
+    else{
+        return x;}
+        
+}
+
+/// ***************************************************
+//                   Sign Extend
+// ***************************************************
+
+signed int SignExtend(int x){
+        
+        if(x&0x00008000){
+            x = x|0xFFFF0000;
+        }
+        
+        return x;
+}
+
+
+
+// ==============================================================================================================
+// ==============================================================================================================
+
+
+// *********************************************************************
+//                        MAIN FUNCTION
+// *********************************************************************
+
+int main(){
+   
+   // Initialization
+
+   DE0_Init(0x8002);                                                            
+   L_MotorInit();
+   L_integrator = 0;
+   ControlInterrupt.attach(&ControlThread, 0.0005);
+   
+   
+   // Specify Setpoint ( rads/sec )
+
+    setpoint = UserInput();
+   
+   // Display Global Variables to Console
+   
+   while(1){
+       
+       float error_t = e;
+       float u_t = u;
+       
+       printf("\n\r US : %i",setpoint);      // User defined setpoint
+       printf("\n\r VE : %2.2f",error_t);             // Displays signed Velocity Error
+       printf("\n\r IS : %i",L_integrator);  // Displays currently state of the left integrator
+       printf("\n\r CS : %2.4f",u_t);             // Displays control signal
+       printf("\n\r\n\r");
+       wait(0.75);
+   }
+   
+}