USNA WRC EW306 / Mbed 2 deprecated dc_motor_interface

Working Version of DC Motor Position Control Code

Dependencies:   mbed mbedWSEsbc2

main.cpp@1:f2a984023592, 2018-04-03 (annotated)

Committer:
jdawkins
Date:
Tue Apr 03 20:14:42 2018 +0000
Revision:
1:f2a984023592
Parent:
0:66d0c1df2bc4 
Position Control of DC Motor Instructor Code

Who changed what in which revision?

UserRevisionLine numberNew 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 }