Harrison Kubena / Mbed 2 deprecated ES306_Motor

a

Dependencies:   mbed mbedWSEsbc

main.cpp@1:ca98a3ae7a70, 2018-04-23 (annotated)

Committer:
m193516
Date:
Mon Apr 23 01:31:28 2018 +0000
Revision:
1:ca98a3ae7a70
Parent:
0:40605c19e1a6 
Motor Experiment

Who changed what in which revision?

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