Diff: main.cpp

Revision:

0:66d0c1df2bc4

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Tue Apr 03 20:11:12 2018 +0000
@@ -0,0 +1,154 @@
+/*************************************************************************************
+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 dc_esf; // duty cycle computed by ESF control law
+float dc; // duty cycle applied to motor
+float dc_min = 0.05; // dead zone limit
+float dc_comp; // compensation for dead zone
+long enc1; // encoder variable
+float des_ang;
+float da;
+int Ncts; // number of counts = TotalTime/1.0; Ts = 1.0;
+int cts; // running counter
+
+// SF gains
+float K1;
+float K2;
+float Kcal;
+
+// Observer matrices
+float Ad11;
+float Ad12;
+float Ad21;
+float Ad22;
+float bd1;
+float bd2;
+float Ld1;
+float Ld2;
+
+
+void ctrlr_test ()
+{
+    // 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;}  
+    
+    // saturation
+    if (dc > 1.0) {
+        dc = 1.0;
+    }
+
+    if (dc < -1.0) {
+        dc = -1.0;
+    }
+    // observer
+    speed_est = Ad11*speed_est_prev + Ad12*ang_est_prev + bd1*ang;
+    ang_est = Ad21*speed_est_prev + Ad22*ang_est_prev + bd2*ang;
+    //speed_est = 0*speed_est_prev + (-1/Ts)*ang_est_prev + (1/Ts)*ang;
+    //ang_est = 0*speed_est_prev + 0*ang_est_prev + 1*ang;
+
+    mot_control(1,dc);
+}
+
+void ctrlr_esf ()
+{
+    // 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
+    // dead zone (static friction) compensation
+
+    if (dc > 0) {
+        dc_comp = dc_min;
+    } else if (dc < 0) {
+        dc_comp = -dc_min;
+    } else {
+        dc_comp = 0;
+    }
+                   
+    if (Time <0.1)
+        {da = 0;}
+    else if (Time >0.1 && Time < 1.1){
+    da = 1;
+    }else{
+    da = 0;    
+    }
+    dc_esf = -K1*speed_est-K2*ang_est+Kcal*da*des_ang;
+    dc = dc_esf + da*dc_comp;
+    // dc = dc_esf;
+    
+    // saturation
+    if (dc > 1.0) {
+        dc = 1.0;
+    }
+
+    if (dc < -1.0) {
+        dc = -1.0;
+    }
+    // observer
+    speed_est = Ad11*speed_est_prev + Ad12*ang_est_prev + bd1*dc + Ld1*ang;
+    ang_est = Ad21*speed_est_prev + Ad22*ang_est_prev + bd2*dc + Ld2*ang;
+    //speed_est = 0*speed_est_prev + (-1/Ts)*ang_est_prev + (1/Ts)*ang;
+    //ang_est = 0*speed_est_prev + 0*ang_est_prev + 1*ang;
+
+    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(&ctrlr_esf,Ts); // run ctrlr function every Ts sec
+        //{ctrlr.attach(&ctrlr_test,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);
+    }
+}