Diff: main.cpp

Revision:

0:ae92c1cda9e8

diff -r 000000000000 -r ae92c1cda9e8 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Apr 12 19:23:01 2018 +0000
@@ -0,0 +1,163 @@
+/*************************************************************************************
+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 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
+
+
+// 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 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;
+}
+
+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;
+    
+    // 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    
+   
+
+    
+    // Control Law based on estimates
+    
+
+    // dead zone (static friction) compensation
+
+
+    // Convert voltage to duty cycle
+
+    
+    // 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;
+        speed_est_prev = 0.0;
+        ang_est_prev = 0.0;
+        speed_est = 0.0;
+        ang_est = 0.0;                
+        LS7366_reset_counter(1);                
+        
+        ctrlr.attach(&open_loop_ctrl,Ts);// run ctrlr function every Ts sec        
+        //ctrlr.attach(&closed_loop_ctrl,Ts); // run ctrlr function every Ts sec
+
+  
+        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