Revision 1:ca98a3ae7a70, committed 2018-04-23

Comitter:

m193516

Date:

Mon Apr 23 01:31:28 2018 +0000

Parent:

0:40605c19e1a6

Commit message:

Motor Experiment

Changed in this revision

diff -r 40605c19e1a6 -r ca98a3ae7a70 main.cpp
--- a/main.cpp	Mon Nov 06 03:04:52 2017 +0000
+++ b/main.cpp	Mon Apr 23 01:31:28 2018 +0000
@@ -1,90 +1,176 @@
-/* Harrison Kubena and Rav Dhingra
-ES305 Lab 1 - Motor Ticker
-27 August 2017 */
+/*************************************************************************************
+Program Name: ES306 Laboratory Experiment  - mbed serial setup and streaming
+Description: A basic code to read an analog channel and print the data to the serial port
+Author: Rich O'Brien, PhD, USNA
+Date: 13 Mar 2018
+**************************************************************************************/
 
+// Include necessary libraries
 #include "mbed.h"
 #include "mbedWSEsbc.h"
 #define PI (3.14159)
 
-//declare objects
-Ticker Controller; //declare ticker obj
+// Declare necessary objects
+DigitalOut myled(LED1);
+Ticker ctrlr;
+
+// variables for data handling and storage
+float Ts = 0.01;    // Sampling period 1/Ts Hz
+float TotalTime;    // Total run time
+float Time;         // elapsed time
+float ang;          // position measured by encoder
+float ang_est;      // observer states
+float speed_est;    // observer states
+float ang_est_prev; // previous observer states
+float speed_est_prev; // previous observer states
+float volts;        // voltage computed by control law
+float dc;           // duty cycle applied to motor
+float dc_comp;      // compensation for dead zone
+long enc1;          // encoder variable
+float des_ang;      // Desired Angle Variable
+float r;            // reference For controller
+int Ncts; // number of counts = TotalTime/1.0; Ts = 1.0;
+int cts; // running counter
+
+// SF gains
+float K1;           // speed gain
+float K2;           // position gain
+float Kcal;         // calibraiton gain
+
 
-//declare variables
-float TotalTime;
-float Time;
-float Tsamp = 0.0083;
-float Tstream = 0.01;
-float userDC;
-float ang, angp, speed;
-float dc;
-float enc1;
-float lowDC;
+// Observer matrix variables
+float Ad11;     //top left
+float Ad12;     // top right
+float Ad21;     // bottom left
+float Ad22;     // bottom right
+float bd1;      //top
+float bd2;      //bottom
+float Ld1;      //top
+float Ld2;      //bottom
+
 
-void ctrCode();
-void twoStepCode();
+void open_loop_ctrl()
+{
+    // Read encoder
+    enc1 = LS7366_read_counter(1); // input is the encoder channel
+    // Convert from counts to radians
+    ang = 2*PI*enc1/6500.0;
+    
+    // duty cycle
+    if (Time <0.75)
+    {dc = 0.1;}
+    else {dc = -0.1;}  
+    
+    //dc = des_ang;
+    // saturation
+    if (dc > 1.0) {
+        dc = 1.0;
+    }
+
+    if (dc < -1.0) {
+        dc = -1.0;
+    }
+    
+    // Observer Equations for spd_est and ang_est
+    
+    speed_est = Ad11*speed_est_prev + Ad12*ang_est_prev + bd1*volts + Ld1*ang;
+    ang_est = Ad21*speed_est_prev + Ad22*ang_est_prev + bd2*volts + Ld2*ang;
+    
+    // Send current Duty Cycle
+    mot_control(1,dc);
+    
+    // Age Variables
+    speed_est_prev = speed_est;
+    ang_est_prev = ang_est;
+} 
 
-//enter main
-int main()  {  
-    //init mbed
+void closed_loop_ctrl()
+{
+    // Read encoder
+    enc1 = LS7366_read_counter(1); // input is the encoder channel
+    // Convert from counts to radians
+    ang = 2*PI*enc1/6500.0;
     
-    mbedWSEsbcInit(115200);
-    mot_en1.period(0.020);
+    // Logic to set Desired Angle
+    if (Time <0.1){
+        r = 0;        
+    }
+    else if (Time >0.1 && Time < 1.1){
+        r = des_ang;
+    }else{
+        r = 0;    
+    }    
+    
+    
+    // Observer Equations for spd_est and ang_est    
+   
+    speed_est = Ad11*speed_est_prev + Ad12*ang_est_prev + bd1*volts + Ld1*ang;
+    ang_est = Ad21*speed_est_prev + Ad22*ang_est_prev + bd2*volts + Ld2*ang;
+    
+    // Control Law based on estimates
+    
+    volts = Kcal*r - K1*speed_est - K2*ang_est;
+
+    // Convert voltage to duty cycle
+    
+    dc = volts/20;
+    
+    // dead zone (static friction) compensation
     
-    while(1)    {
-        
-        pc.scanf("%f,%f", &TotalTime, &lowDC);
+    dc_comp = 0.06; // static friction value
+    /*
+    if (dc > 0 && abs(dc) < dc_comp) {
+        dc = dc_comp;
+    }
+    
+    if (dc < 0 && abs(dc) < dc_comp) {
+        dc = -1*dc_comp;
+    }
+    */   
+
+    // saturation
+    if (dc > 1.0) {
+        dc = 1.0;
+    }
+
+    if (dc < -1.0) {
+        dc = -1.0;
+    }
+
+
+    mot_control(1,dc);
+    speed_est_prev = speed_est;
+    ang_est_prev = ang_est;
+}
+
+
+int main ()
+{
+    mbedWSEsbcInit(115200);
+    mot_en1.period(.020);
+    while(1) { // repeat collection cycle indefinitely
+        pc.scanf("%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f,%f",&TotalTime,&des_ang,&K1,&K2,&Kcal,&Ad11,&Ad12,&Ad21,&Ad22,&bd1,&bd2,&Ld1,&Ld2);
+        Ncts = floor(TotalTime/Ts);
+         cts = 0;
         Time = 0.0;
-        t.reset();
-        Controller.attach(&twoStepCode,Tsamp);
-        t.start();
-        
-        //data streaming
+        speed_est_prev = 0.0;
+        ang_est_prev = 0.0;
+        speed_est = 0.0;
+        ang_est = 0.0;                
+        LS7366_reset_counter(1);                
         
-        while(Time <= TotalTime)    {
-            
-            Time = t.read();
-            pc.printf("%f, %f, %f\n", Time, speed, dc);
-            wait(Tstream);
-                                    }
-            Controller.detach();
-            mot_control(1,0.0);
-                }
-            }  
-// additional function definitions
-void ctrCode()  {
-        //read encoder
-        enc1 = LS7366_read_counter(1);
-        //convert counts to rads
-        ang = 2.0*PI*enc1/6400.0;
-        //estimate speed
-        speed = (ang-angp)/Tsamp;
-        //age variables
-        angp = ang;
-        //compute duty cycle
-        dc = userDC;
-        //send the duty cycle to motor
-        mot_control(1,dc); }
-        
-void twoStepCode()  {
-        //read encoder
-        enc1 = LS7366_read_counter(1);
-        //convert counts to rads
-        ang = 2.0*PI*enc1/6400.0;
-        //estimate speed
-        speed = (ang-angp)/Tsamp;
-        //age variables
-        angp = ang;
-        //compute duty cycle
-        if(Time<0.1){
-            dc = 0.0;
-            } else if(Time<0.55){
-            dc = lowDC;
-            } else {
-            dc = 0.10;
-            }   
-        //send the duty cycle to motor
-        mot_control(1,dc); }
-                
-                
+        // ctrlr.attach(&open_loop_ctrl,Ts);// run ctrlr function every Ts sec        
+        ctrlr.attach(&closed_loop_ctrl,Ts); // run ctrlr function every Ts sec
 
-        
\ No newline at end of file
+  
+        while(cts <= Ncts) {
+            pc.printf("%f,%f,%f,%f,%f\n",Time,ang,ang_est,speed_est,dc);
+            cts = cts + 1;
+            Time = Time + Ts;
+            wait(Ts);
+
+        } // end while (cts <= Ncts)
+        ctrlr.detach(); 
+        mot_control(1,0);
+    }
+}
\ No newline at end of file