Charlene Bissell
/
306_DCexp
For Harrison
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?
User | Revision | Line number | New 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 | } |