Working Version of DC Motor Position Control Code
Dependencies: mbed mbedWSEsbc2
main.cpp
- Committer:
- jdawkins
- Date:
- 2018-04-03
- Revision:
- 0:66d0c1df2bc4
File content as of revision 0:66d0c1df2bc4:
/************************************************************************************* 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); } }