File content as of revision 1:ca98a3ae7a70:

/*************************************************************************************
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    
   
    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
    
    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;
        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);
    }
}