Working Version of DC Motor Position Control Code
Dependencies: mbed mbedWSEsbc2
Diff: main.cpp
- Revision:
- 0:66d0c1df2bc4
diff -r 000000000000 -r 66d0c1df2bc4 main.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Tue Apr 03 20:11:12 2018 +0000 @@ -0,0 +1,154 @@ +/************************************************************************************* +Program Name: ES306 Laboratory Experiment - mbed serial setup and streaming +Description: A basic code to read an analog channel and print the data to the serial port +Author: Rich O'Brien, PhD, USNA +Date: 13 Mar 2018 +**************************************************************************************/ + +// Include necessary libraries +#include "mbed.h" +#include "mbedWSEsbc.h" +#define PI (3.14159) + +// Declare necessary objects +DigitalOut myled(LED1); +Ticker ctrlr; + +// variables for data handling and storage +float Ts = 0.01; // Sampling period 1/Ts Hz +float TotalTime; // Total run time +float Time; // elapsed time +float ang; // position measured by encoder +float ang_est; // observer states +float speed_est; // observer states +float ang_est_prev; // previous observer states +float speed_est_prev; // previous observer states +float dc_esf; // duty cycle computed by ESF control law +float dc; // duty cycle applied to motor +float dc_min = 0.05; // dead zone limit +float dc_comp; // compensation for dead zone +long enc1; // encoder variable +float des_ang; +float da; +int Ncts; // number of counts = TotalTime/1.0; Ts = 1.0; +int cts; // running counter + +// SF gains +float K1; +float K2; +float Kcal; + +// Observer matrices +float Ad11; +float Ad12; +float Ad21; +float Ad22; +float bd1; +float bd2; +float Ld1; +float Ld2; + + +void ctrlr_test () +{ + // Read encoder + enc1 = LS7366_read_counter(1); // input is the encoder channel + // Convert from counts to radians + ang = 2*PI*enc1/6500.0; + // duty cycle + if (Time <0.75) + {dc = 0.1;} + else {dc = -0.1;} + + // saturation + if (dc > 1.0) { + dc = 1.0; + } + + if (dc < -1.0) { + dc = -1.0; + } + // observer + speed_est = Ad11*speed_est_prev + Ad12*ang_est_prev + bd1*ang; + ang_est = Ad21*speed_est_prev + Ad22*ang_est_prev + bd2*ang; + //speed_est = 0*speed_est_prev + (-1/Ts)*ang_est_prev + (1/Ts)*ang; + //ang_est = 0*speed_est_prev + 0*ang_est_prev + 1*ang; + + mot_control(1,dc); +} + +void ctrlr_esf () +{ + // Read encoder + enc1 = LS7366_read_counter(1); // input is the encoder channel + // Convert from counts to radians + ang = 2*PI*enc1/6500.0; + // duty cycle + // dead zone (static friction) compensation + + if (dc > 0) { + dc_comp = dc_min; + } else if (dc < 0) { + dc_comp = -dc_min; + } else { + dc_comp = 0; + } + + if (Time <0.1) + {da = 0;} + else if (Time >0.1 && Time < 1.1){ + da = 1; + }else{ + da = 0; + } + dc_esf = -K1*speed_est-K2*ang_est+Kcal*da*des_ang; + dc = dc_esf + da*dc_comp; + // dc = dc_esf; + + // saturation + if (dc > 1.0) { + dc = 1.0; + } + + if (dc < -1.0) { + dc = -1.0; + } + // observer + speed_est = Ad11*speed_est_prev + Ad12*ang_est_prev + bd1*dc + Ld1*ang; + ang_est = Ad21*speed_est_prev + Ad22*ang_est_prev + bd2*dc + Ld2*ang; + //speed_est = 0*speed_est_prev + (-1/Ts)*ang_est_prev + (1/Ts)*ang; + //ang_est = 0*speed_est_prev + 0*ang_est_prev + 1*ang; + + mot_control(1,dc); + speed_est_prev = speed_est; + ang_est_prev = ang_est; +} + +int main () +{ + mbedWSEsbcInit(115200); + mot_en1.period(.020); + while(1) { // repeat collection cycle indefinitely + 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); + Ncts = floor(TotalTime/Ts); + cts = 0; + Time = 0.0; + speed_est_prev = 0.0; + ang_est_prev = 0.0; + speed_est = 0.0; + ang_est = 0.0; + LS7366_reset_counter(1); + ctrlr.attach(&ctrlr_esf,Ts); // run ctrlr function every Ts sec + //{ctrlr.attach(&ctrlr_test,Ts);// run ctrlr function every Ts sec + + while(cts <= Ncts) { + pc.printf("%f,%f,%f,%f,%f\n",Time,ang,ang_est,speed_est,dc); + cts = cts + 1; + Time = Time + Ts; + wait(Ts); + + } // end while (cts <= Ncts) + ctrlr.detach(); + mot_control(1,0); + } +}