Charlene Bissell / Mbed 2 deprecated 306_DCexp

For Harrison

Dependencies:   mbed mbedWSEsbc

main.cpp@0:ae92c1cda9e8, 2018-04-12 (annotated)

Committer:
m190516
Date:
Thu Apr 12 19:23:01 2018 +0000
Revision:
0:ae92c1cda9e8
lol

Who changed what in which revision?

UserRevisionLine numberNew contents of line
m190516 0:ae92c1cda9e8 1 /*************************************************************************************
m190516 0:ae92c1cda9e8 2 Program Name: ES306 Laboratory Experiment - mbed serial setup and streaming
m190516 0:ae92c1cda9e8 3 Description: A basic code to read an analog channel and print the data to the serial port
m190516 0:ae92c1cda9e8 4 Author: Rich O'Brien, PhD, USNA
m190516 0:ae92c1cda9e8 5 Date: 13 Mar 2018
m190516 0:ae92c1cda9e8 6 **************************************************************************************/
m190516 0:ae92c1cda9e8 7
m190516 0:ae92c1cda9e8 8 // Include necessary libraries
m190516 0:ae92c1cda9e8 9 #include "mbed.h"
m190516 0:ae92c1cda9e8 10 #include "mbedWSEsbc.h"
m190516 0:ae92c1cda9e8 11 #define PI (3.14159)
m190516 0:ae92c1cda9e8 12
m190516 0:ae92c1cda9e8 13 // Declare necessary objects
m190516 0:ae92c1cda9e8 14 DigitalOut myled(LED1);
m190516 0:ae92c1cda9e8 15 Ticker ctrlr;
m190516 0:ae92c1cda9e8 16
m190516 0:ae92c1cda9e8 17 // variables for data handling and storage
m190516 0:ae92c1cda9e8 18 float Ts = 0.01; // Sampling period 1/Ts Hz
m190516 0:ae92c1cda9e8 19 float TotalTime; // Total run time
m190516 0:ae92c1cda9e8 20 float Time; // elapsed time
m190516 0:ae92c1cda9e8 21 float ang; // position measured by encoder
m190516 0:ae92c1cda9e8 22 float ang_est; // observer states
m190516 0:ae92c1cda9e8 23 float speed_est; // observer states
m190516 0:ae92c1cda9e8 24 float ang_est_prev; // previous observer states
m190516 0:ae92c1cda9e8 25 float speed_est_prev; // previous observer states
m190516 0:ae92c1cda9e8 26 float volts; // voltage computed by control law
m190516 0:ae92c1cda9e8 27 float dc; // duty cycle applied to motor
m190516 0:ae92c1cda9e8 28 float dc_comp; // compensation for dead zone
m190516 0:ae92c1cda9e8 29 long enc1; // encoder variable
m190516 0:ae92c1cda9e8 30 float des_ang; // Desired Angle Variable
m190516 0:ae92c1cda9e8 31 float r; // reference For controller
m190516 0:ae92c1cda9e8 32 int Ncts; // number of counts = TotalTime/1.0; Ts = 1.0;
m190516 0:ae92c1cda9e8 33 int cts; // running counter
m190516 0:ae92c1cda9e8 34
m190516 0:ae92c1cda9e8 35 // SF gains
m190516 0:ae92c1cda9e8 36 float K1; // speed gain
m190516 0:ae92c1cda9e8 37 float K2; // position gain
m190516 0:ae92c1cda9e8 38 float Kcal; // calibraiton gain
m190516 0:ae92c1cda9e8 39
m190516 0:ae92c1cda9e8 40
m190516 0:ae92c1cda9e8 41 // Observer matrix variables
m190516 0:ae92c1cda9e8 42 float Ad11; //top left
m190516 0:ae92c1cda9e8 43 float Ad12; // top right
m190516 0:ae92c1cda9e8 44 float Ad21; // bottom left
m190516 0:ae92c1cda9e8 45 float Ad22; // bottom right
m190516 0:ae92c1cda9e8 46 float bd1; //top
m190516 0:ae92c1cda9e8 47 float bd2; //bottom
m190516 0:ae92c1cda9e8 48 float Ld1; //top
m190516 0:ae92c1cda9e8 49 float Ld2; //bottom
m190516 0:ae92c1cda9e8 50
m190516 0:ae92c1cda9e8 51
m190516 0:ae92c1cda9e8 52 void open_loop_ctrl()
m190516 0:ae92c1cda9e8 53 {
m190516 0:ae92c1cda9e8 54 // Read encoder
m190516 0:ae92c1cda9e8 55 enc1 = LS7366_read_counter(1); // input is the encoder channel
m190516 0:ae92c1cda9e8 56 // Convert from counts to radians
m190516 0:ae92c1cda9e8 57 ang = 2*PI*enc1/6500.0;
m190516 0:ae92c1cda9e8 58
m190516 0:ae92c1cda9e8 59 // duty cycle
m190516 0:ae92c1cda9e8 60 if (Time <0.75)
m190516 0:ae92c1cda9e8 61 {dc = 0.1;}
m190516 0:ae92c1cda9e8 62 else {dc = -0.1;}
m190516 0:ae92c1cda9e8 63
m190516 0:ae92c1cda9e8 64 //dc = des_ang;
m190516 0:ae92c1cda9e8 65 // saturation
m190516 0:ae92c1cda9e8 66 if (dc > 1.0) {
m190516 0:ae92c1cda9e8 67 dc = 1.0;
m190516 0:ae92c1cda9e8 68 }
m190516 0:ae92c1cda9e8 69
m190516 0:ae92c1cda9e8 70 if (dc < -1.0) {
m190516 0:ae92c1cda9e8 71 dc = -1.0;
m190516 0:ae92c1cda9e8 72 }
m190516 0:ae92c1cda9e8 73
m190516 0:ae92c1cda9e8 74 // Observer Equations for spd_est and ang_est
m190516 0:ae92c1cda9e8 75
m190516 0:ae92c1cda9e8 76 speed_est = Ad11*speed_est_prev+Ad12*ang_est_prev+bd1*volts+Ld1*ang;
m190516 0:ae92c1cda9e8 77 ang_est = Ad21*speed_est_prev+Ad22*ang_est_prev+bd2*volts+Ld2*ang;
m190516 0:ae92c1cda9e8 78
m190516 0:ae92c1cda9e8 79 // Send current Duty Cycle
m190516 0:ae92c1cda9e8 80 mot_control(1,dc);
m190516 0:ae92c1cda9e8 81
m190516 0:ae92c1cda9e8 82 // Age Variables
m190516 0:ae92c1cda9e8 83 speed_est_prev = speed_est;
m190516 0:ae92c1cda9e8 84 ang_est_prev = ang_est;
m190516 0:ae92c1cda9e8 85 }
m190516 0:ae92c1cda9e8 86
m190516 0:ae92c1cda9e8 87 void closed_loop_ctrl()
m190516 0:ae92c1cda9e8 88 {
m190516 0:ae92c1cda9e8 89 // Read encoder
m190516 0:ae92c1cda9e8 90 enc1 = LS7366_read_counter(1); // input is the encoder channel
m190516 0:ae92c1cda9e8 91 // Convert from counts to radians
m190516 0:ae92c1cda9e8 92 ang = 2*PI*enc1/6500.0;
m190516 0:ae92c1cda9e8 93
m190516 0:ae92c1cda9e8 94 // Logic to set Desired Angle
m190516 0:ae92c1cda9e8 95 if (Time <0.1){
m190516 0:ae92c1cda9e8 96 r = 0;
m190516 0:ae92c1cda9e8 97 }
m190516 0:ae92c1cda9e8 98 else if (Time >0.1 && Time < 1.1){
m190516 0:ae92c1cda9e8 99 r = des_ang;
m190516 0:ae92c1cda9e8 100 }else{
m190516 0:ae92c1cda9e8 101 r = 0;
m190516 0:ae92c1cda9e8 102 }
m190516 0:ae92c1cda9e8 103
m190516 0:ae92c1cda9e8 104
m190516 0:ae92c1cda9e8 105 // Observer Equations for spd_est and ang_est
m190516 0:ae92c1cda9e8 106
m190516 0:ae92c1cda9e8 107
m190516 0:ae92c1cda9e8 108
m190516 0:ae92c1cda9e8 109 // Control Law based on estimates
m190516 0:ae92c1cda9e8 110
m190516 0:ae92c1cda9e8 111
m190516 0:ae92c1cda9e8 112 // dead zone (static friction) compensation
m190516 0:ae92c1cda9e8 113
m190516 0:ae92c1cda9e8 114
m190516 0:ae92c1cda9e8 115 // Convert voltage to duty cycle
m190516 0:ae92c1cda9e8 116
m190516 0:ae92c1cda9e8 117
m190516 0:ae92c1cda9e8 118 // saturation
m190516 0:ae92c1cda9e8 119 if (dc > 1.0) {
m190516 0:ae92c1cda9e8 120 dc = 1.0;
m190516 0:ae92c1cda9e8 121 }
m190516 0:ae92c1cda9e8 122
m190516 0:ae92c1cda9e8 123 if (dc < -1.0) {
m190516 0:ae92c1cda9e8 124 dc = -1.0;
m190516 0:ae92c1cda9e8 125 }
m190516 0:ae92c1cda9e8 126
m190516 0:ae92c1cda9e8 127
m190516 0:ae92c1cda9e8 128 mot_control(1,dc);
m190516 0:ae92c1cda9e8 129 speed_est_prev = speed_est;
m190516 0:ae92c1cda9e8 130 ang_est_prev = ang_est;
m190516 0:ae92c1cda9e8 131 }
m190516 0:ae92c1cda9e8 132
m190516 0:ae92c1cda9e8 133
m190516 0:ae92c1cda9e8 134 int main ()
m190516 0:ae92c1cda9e8 135 {
m190516 0:ae92c1cda9e8 136 mbedWSEsbcInit(115200);
m190516 0:ae92c1cda9e8 137 mot_en1.period(.020);
m190516 0:ae92c1cda9e8 138 while(1) { // repeat collection cycle indefinitely
m190516 0:ae92c1cda9e8 139 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);
m190516 0:ae92c1cda9e8 140 Ncts = floor(TotalTime/Ts);
m190516 0:ae92c1cda9e8 141 cts = 0;
m190516 0:ae92c1cda9e8 142 Time = 0.0;
m190516 0:ae92c1cda9e8 143 speed_est_prev = 0.0;
m190516 0:ae92c1cda9e8 144 ang_est_prev = 0.0;
m190516 0:ae92c1cda9e8 145 speed_est = 0.0;
m190516 0:ae92c1cda9e8 146 ang_est = 0.0;
m190516 0:ae92c1cda9e8 147 LS7366_reset_counter(1);
m190516 0:ae92c1cda9e8 148
m190516 0:ae92c1cda9e8 149 ctrlr.attach(&open_loop_ctrl,Ts);// run ctrlr function every Ts sec
m190516 0:ae92c1cda9e8 150 //ctrlr.attach(&closed_loop_ctrl,Ts); // run ctrlr function every Ts sec
m190516 0:ae92c1cda9e8 151
m190516 0:ae92c1cda9e8 152
m190516 0:ae92c1cda9e8 153 while(cts <= Ncts) {
m190516 0:ae92c1cda9e8 154 pc.printf("%f,%f,%f,%f,%f\n",Time,ang,ang_est,speed_est,dc);
m190516 0:ae92c1cda9e8 155 cts = cts + 1;
m190516 0:ae92c1cda9e8 156 Time = Time + Ts;
m190516 0:ae92c1cda9e8 157 wait(Ts);
m190516 0:ae92c1cda9e8 158
m190516 0:ae92c1cda9e8 159 } // end while (cts <= Ncts)
m190516 0:ae92c1cda9e8 160 ctrlr.detach();
m190516 0:ae92c1cda9e8 161 mot_control(1,0);
m190516 0:ae92c1cda9e8 162 }
m190516 0:ae92c1cda9e8 163 }