Working Version of DC Motor Position Control Code
Dependencies: mbed mbedWSEsbc2
main.cpp@0:66d0c1df2bc4, 2018-04-03 (annotated)
- Committer:
- jdawkins
- Date:
- Tue Apr 03 20:11:12 2018 +0000
- Revision:
- 0:66d0c1df2bc4
Add Commit to Group
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
jdawkins | 0:66d0c1df2bc4 | 1 | /************************************************************************************* |
jdawkins | 0:66d0c1df2bc4 | 2 | Program Name: ES306 Laboratory Experiment - mbed serial setup and streaming |
jdawkins | 0:66d0c1df2bc4 | 3 | Description: A basic code to read an analog channel and print the data to the serial port |
jdawkins | 0:66d0c1df2bc4 | 4 | Author: Rich O'Brien, PhD, USNA |
jdawkins | 0:66d0c1df2bc4 | 5 | Date: 13 Mar 2018 |
jdawkins | 0:66d0c1df2bc4 | 6 | **************************************************************************************/ |
jdawkins | 0:66d0c1df2bc4 | 7 | |
jdawkins | 0:66d0c1df2bc4 | 8 | // Include necessary libraries |
jdawkins | 0:66d0c1df2bc4 | 9 | #include "mbed.h" |
jdawkins | 0:66d0c1df2bc4 | 10 | #include "mbedWSEsbc.h" |
jdawkins | 0:66d0c1df2bc4 | 11 | #define PI (3.14159) |
jdawkins | 0:66d0c1df2bc4 | 12 | |
jdawkins | 0:66d0c1df2bc4 | 13 | // Declare necessary objects |
jdawkins | 0:66d0c1df2bc4 | 14 | DigitalOut myled(LED1); |
jdawkins | 0:66d0c1df2bc4 | 15 | Ticker ctrlr; |
jdawkins | 0:66d0c1df2bc4 | 16 | |
jdawkins | 0:66d0c1df2bc4 | 17 | // variables for data handling and storage |
jdawkins | 0:66d0c1df2bc4 | 18 | float Ts = 0.01; // Sampling period 1/Ts Hz |
jdawkins | 0:66d0c1df2bc4 | 19 | float TotalTime; // Total run time |
jdawkins | 0:66d0c1df2bc4 | 20 | float Time; // elapsed time |
jdawkins | 0:66d0c1df2bc4 | 21 | float ang; // position measured by encoder |
jdawkins | 0:66d0c1df2bc4 | 22 | float ang_est; // observer states |
jdawkins | 0:66d0c1df2bc4 | 23 | float speed_est; // observer states |
jdawkins | 0:66d0c1df2bc4 | 24 | float ang_est_prev; // previous observer states |
jdawkins | 0:66d0c1df2bc4 | 25 | float speed_est_prev; // previous observer states |
jdawkins | 0:66d0c1df2bc4 | 26 | float dc_esf; // duty cycle computed by ESF control law |
jdawkins | 0:66d0c1df2bc4 | 27 | float dc; // duty cycle applied to motor |
jdawkins | 0:66d0c1df2bc4 | 28 | float dc_min = 0.05; // dead zone limit |
jdawkins | 0:66d0c1df2bc4 | 29 | float dc_comp; // compensation for dead zone |
jdawkins | 0:66d0c1df2bc4 | 30 | long enc1; // encoder variable |
jdawkins | 0:66d0c1df2bc4 | 31 | float des_ang; |
jdawkins | 0:66d0c1df2bc4 | 32 | float da; |
jdawkins | 0:66d0c1df2bc4 | 33 | int Ncts; // number of counts = TotalTime/1.0; Ts = 1.0; |
jdawkins | 0:66d0c1df2bc4 | 34 | int cts; // running counter |
jdawkins | 0:66d0c1df2bc4 | 35 | |
jdawkins | 0:66d0c1df2bc4 | 36 | // SF gains |
jdawkins | 0:66d0c1df2bc4 | 37 | float K1; |
jdawkins | 0:66d0c1df2bc4 | 38 | float K2; |
jdawkins | 0:66d0c1df2bc4 | 39 | float Kcal; |
jdawkins | 0:66d0c1df2bc4 | 40 | |
jdawkins | 0:66d0c1df2bc4 | 41 | // Observer matrices |
jdawkins | 0:66d0c1df2bc4 | 42 | float Ad11; |
jdawkins | 0:66d0c1df2bc4 | 43 | float Ad12; |
jdawkins | 0:66d0c1df2bc4 | 44 | float Ad21; |
jdawkins | 0:66d0c1df2bc4 | 45 | float Ad22; |
jdawkins | 0:66d0c1df2bc4 | 46 | float bd1; |
jdawkins | 0:66d0c1df2bc4 | 47 | float bd2; |
jdawkins | 0:66d0c1df2bc4 | 48 | float Ld1; |
jdawkins | 0:66d0c1df2bc4 | 49 | float Ld2; |
jdawkins | 0:66d0c1df2bc4 | 50 | |
jdawkins | 0:66d0c1df2bc4 | 51 | |
jdawkins | 0:66d0c1df2bc4 | 52 | void ctrlr_test () |
jdawkins | 0:66d0c1df2bc4 | 53 | { |
jdawkins | 0:66d0c1df2bc4 | 54 | // Read encoder |
jdawkins | 0:66d0c1df2bc4 | 55 | enc1 = LS7366_read_counter(1); // input is the encoder channel |
jdawkins | 0:66d0c1df2bc4 | 56 | // Convert from counts to radians |
jdawkins | 0:66d0c1df2bc4 | 57 | ang = 2*PI*enc1/6500.0; |
jdawkins | 0:66d0c1df2bc4 | 58 | // duty cycle |
jdawkins | 0:66d0c1df2bc4 | 59 | if (Time <0.75) |
jdawkins | 0:66d0c1df2bc4 | 60 | {dc = 0.1;} |
jdawkins | 0:66d0c1df2bc4 | 61 | else {dc = -0.1;} |
jdawkins | 0:66d0c1df2bc4 | 62 | |
jdawkins | 0:66d0c1df2bc4 | 63 | // saturation |
jdawkins | 0:66d0c1df2bc4 | 64 | if (dc > 1.0) { |
jdawkins | 0:66d0c1df2bc4 | 65 | dc = 1.0; |
jdawkins | 0:66d0c1df2bc4 | 66 | } |
jdawkins | 0:66d0c1df2bc4 | 67 | |
jdawkins | 0:66d0c1df2bc4 | 68 | if (dc < -1.0) { |
jdawkins | 0:66d0c1df2bc4 | 69 | dc = -1.0; |
jdawkins | 0:66d0c1df2bc4 | 70 | } |
jdawkins | 0:66d0c1df2bc4 | 71 | // observer |
jdawkins | 0:66d0c1df2bc4 | 72 | speed_est = Ad11*speed_est_prev + Ad12*ang_est_prev + bd1*ang; |
jdawkins | 0:66d0c1df2bc4 | 73 | ang_est = Ad21*speed_est_prev + Ad22*ang_est_prev + bd2*ang; |
jdawkins | 0:66d0c1df2bc4 | 74 | //speed_est = 0*speed_est_prev + (-1/Ts)*ang_est_prev + (1/Ts)*ang; |
jdawkins | 0:66d0c1df2bc4 | 75 | //ang_est = 0*speed_est_prev + 0*ang_est_prev + 1*ang; |
jdawkins | 0:66d0c1df2bc4 | 76 | |
jdawkins | 0:66d0c1df2bc4 | 77 | mot_control(1,dc); |
jdawkins | 0:66d0c1df2bc4 | 78 | } |
jdawkins | 0:66d0c1df2bc4 | 79 | |
jdawkins | 0:66d0c1df2bc4 | 80 | void ctrlr_esf () |
jdawkins | 0:66d0c1df2bc4 | 81 | { |
jdawkins | 0:66d0c1df2bc4 | 82 | // Read encoder |
jdawkins | 0:66d0c1df2bc4 | 83 | enc1 = LS7366_read_counter(1); // input is the encoder channel |
jdawkins | 0:66d0c1df2bc4 | 84 | // Convert from counts to radians |
jdawkins | 0:66d0c1df2bc4 | 85 | ang = 2*PI*enc1/6500.0; |
jdawkins | 0:66d0c1df2bc4 | 86 | // duty cycle |
jdawkins | 0:66d0c1df2bc4 | 87 | // dead zone (static friction) compensation |
jdawkins | 0:66d0c1df2bc4 | 88 | |
jdawkins | 0:66d0c1df2bc4 | 89 | if (dc > 0) { |
jdawkins | 0:66d0c1df2bc4 | 90 | dc_comp = dc_min; |
jdawkins | 0:66d0c1df2bc4 | 91 | } else if (dc < 0) { |
jdawkins | 0:66d0c1df2bc4 | 92 | dc_comp = -dc_min; |
jdawkins | 0:66d0c1df2bc4 | 93 | } else { |
jdawkins | 0:66d0c1df2bc4 | 94 | dc_comp = 0; |
jdawkins | 0:66d0c1df2bc4 | 95 | } |
jdawkins | 0:66d0c1df2bc4 | 96 | |
jdawkins | 0:66d0c1df2bc4 | 97 | if (Time <0.1) |
jdawkins | 0:66d0c1df2bc4 | 98 | {da = 0;} |
jdawkins | 0:66d0c1df2bc4 | 99 | else if (Time >0.1 && Time < 1.1){ |
jdawkins | 0:66d0c1df2bc4 | 100 | da = 1; |
jdawkins | 0:66d0c1df2bc4 | 101 | }else{ |
jdawkins | 0:66d0c1df2bc4 | 102 | da = 0; |
jdawkins | 0:66d0c1df2bc4 | 103 | } |
jdawkins | 0:66d0c1df2bc4 | 104 | dc_esf = -K1*speed_est-K2*ang_est+Kcal*da*des_ang; |
jdawkins | 0:66d0c1df2bc4 | 105 | dc = dc_esf + da*dc_comp; |
jdawkins | 0:66d0c1df2bc4 | 106 | // dc = dc_esf; |
jdawkins | 0:66d0c1df2bc4 | 107 | |
jdawkins | 0:66d0c1df2bc4 | 108 | // saturation |
jdawkins | 0:66d0c1df2bc4 | 109 | if (dc > 1.0) { |
jdawkins | 0:66d0c1df2bc4 | 110 | dc = 1.0; |
jdawkins | 0:66d0c1df2bc4 | 111 | } |
jdawkins | 0:66d0c1df2bc4 | 112 | |
jdawkins | 0:66d0c1df2bc4 | 113 | if (dc < -1.0) { |
jdawkins | 0:66d0c1df2bc4 | 114 | dc = -1.0; |
jdawkins | 0:66d0c1df2bc4 | 115 | } |
jdawkins | 0:66d0c1df2bc4 | 116 | // observer |
jdawkins | 0:66d0c1df2bc4 | 117 | speed_est = Ad11*speed_est_prev + Ad12*ang_est_prev + bd1*dc + Ld1*ang; |
jdawkins | 0:66d0c1df2bc4 | 118 | ang_est = Ad21*speed_est_prev + Ad22*ang_est_prev + bd2*dc + Ld2*ang; |
jdawkins | 0:66d0c1df2bc4 | 119 | //speed_est = 0*speed_est_prev + (-1/Ts)*ang_est_prev + (1/Ts)*ang; |
jdawkins | 0:66d0c1df2bc4 | 120 | //ang_est = 0*speed_est_prev + 0*ang_est_prev + 1*ang; |
jdawkins | 0:66d0c1df2bc4 | 121 | |
jdawkins | 0:66d0c1df2bc4 | 122 | mot_control(1,dc); |
jdawkins | 0:66d0c1df2bc4 | 123 | speed_est_prev = speed_est; |
jdawkins | 0:66d0c1df2bc4 | 124 | ang_est_prev = ang_est; |
jdawkins | 0:66d0c1df2bc4 | 125 | } |
jdawkins | 0:66d0c1df2bc4 | 126 | |
jdawkins | 0:66d0c1df2bc4 | 127 | int main () |
jdawkins | 0:66d0c1df2bc4 | 128 | { |
jdawkins | 0:66d0c1df2bc4 | 129 | mbedWSEsbcInit(115200); |
jdawkins | 0:66d0c1df2bc4 | 130 | mot_en1.period(.020); |
jdawkins | 0:66d0c1df2bc4 | 131 | while(1) { // repeat collection cycle indefinitely |
jdawkins | 0:66d0c1df2bc4 | 132 | 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); |
jdawkins | 0:66d0c1df2bc4 | 133 | Ncts = floor(TotalTime/Ts); |
jdawkins | 0:66d0c1df2bc4 | 134 | cts = 0; |
jdawkins | 0:66d0c1df2bc4 | 135 | Time = 0.0; |
jdawkins | 0:66d0c1df2bc4 | 136 | speed_est_prev = 0.0; |
jdawkins | 0:66d0c1df2bc4 | 137 | ang_est_prev = 0.0; |
jdawkins | 0:66d0c1df2bc4 | 138 | speed_est = 0.0; |
jdawkins | 0:66d0c1df2bc4 | 139 | ang_est = 0.0; |
jdawkins | 0:66d0c1df2bc4 | 140 | LS7366_reset_counter(1); |
jdawkins | 0:66d0c1df2bc4 | 141 | ctrlr.attach(&ctrlr_esf,Ts); // run ctrlr function every Ts sec |
jdawkins | 0:66d0c1df2bc4 | 142 | //{ctrlr.attach(&ctrlr_test,Ts);// run ctrlr function every Ts sec |
jdawkins | 0:66d0c1df2bc4 | 143 | |
jdawkins | 0:66d0c1df2bc4 | 144 | while(cts <= Ncts) { |
jdawkins | 0:66d0c1df2bc4 | 145 | pc.printf("%f,%f,%f,%f,%f\n",Time,ang,ang_est,speed_est,dc); |
jdawkins | 0:66d0c1df2bc4 | 146 | cts = cts + 1; |
jdawkins | 0:66d0c1df2bc4 | 147 | Time = Time + Ts; |
jdawkins | 0:66d0c1df2bc4 | 148 | wait(Ts); |
jdawkins | 0:66d0c1df2bc4 | 149 | |
jdawkins | 0:66d0c1df2bc4 | 150 | } // end while (cts <= Ncts) |
jdawkins | 0:66d0c1df2bc4 | 151 | ctrlr.detach(); |
jdawkins | 0:66d0c1df2bc4 | 152 | mot_control(1,0); |
jdawkins | 0:66d0c1df2bc4 | 153 | } |
jdawkins | 0:66d0c1df2bc4 | 154 | } |