Luke Ledva / Mbed 2 deprecated ES305

asdfghj

Dependencies:   mbed mbedWSEsbc

MotorID.cpp@0:eb94c702a103, 2017-11-28 (annotated)

Committer:
m193672
Date:
Tue Nov 28 04:22:29 2017 +0000
Revision:
0:eb94c702a103
asdfghjk

Who changed what in which revision?

UserRevisionLine numberNew contents of line
m193672 0:eb94c702a103 1 /*************************************************************************************
m193672 0:eb94c702a103 2 Program Name: ES305 Laboratory Experiment 1, part 1 - mbed serial setup and streaming
m193672 0:eb94c702a103 3 Description: A basic code to run a DC motor using a Ticker object. The user specifies
m193672 0:eb94c702a103 4 sun rimte, and duty cycle. The program runs at 100 Hz and streams data at 50 Hz
m193672 0:eb94c702a103 5 Author: Levi DeVries, PhD, USNA
m193672 0:eb94c702a103 6 Date: 8/14/2017
m193672 0:eb94c702a103 7 **************************************************************************************/
m193672 0:eb94c702a103 8
m193672 0:eb94c702a103 9 // Include necessary libraries
m193672 0:eb94c702a103 10 #include "mbed.h"
m193672 0:eb94c702a103 11 #include "mbedWSEsbc.h"
m193672 0:eb94c702a103 12 #define PI (3.14159)
m193672 0:eb94c702a103 13
m193672 0:eb94c702a103 14
m193672 0:eb94c702a103 15 // Declare objects (if necessary)
m193672 0:eb94c702a103 16 Ticker Controller; // declare Ticker object named "Controller"
m193672 0:eb94c702a103 17 DigitalOut myled(LED1); // LED, flash lights for debugging
m193672 0:eb94c702a103 18
m193672 0:eb94c702a103 19
m193672 0:eb94c702a103 20 // variables for data handling and storage
m193672 0:eb94c702a103 21 float TotalTime; // Total run time
m193672 0:eb94c702a103 22 float Time; // Current elapsed time
m193672 0:eb94c702a103 23 float Ts = 0.0083; // Control update period (seconds) (100 Hz equivalent)
m193672 0:eb94c702a103 24 float Tstrm = 0.01; // Data streaming period (seconds) (50 Hz equivalent)
m193672 0:eb94c702a103 25 float usrDC; // uaser-specified duty cycle
m193672 0:eb94c702a103 26 float ang,angp,speed; // variables for approximating speed from encoder measurements
m193672 0:eb94c702a103 27 float dc; // duty cycle applied to motor
m193672 0:eb94c702a103 28 long enc1; // encoder variable
m193672 0:eb94c702a103 29 float lowDC;
m193672 0:eb94c702a103 30 float dspd;
m193672 0:eb94c702a103 31 float spdErr;
m193672 0:eb94c702a103 32 float Kp;
m193672 0:eb94c702a103 33 float dc_p = 0.0;
m193672 0:eb94c702a103 34 float spdErr_p = 0.0;
m193672 0:eb94c702a103 35
m193672 0:eb94c702a103 36
m193672 0:eb94c702a103 37
m193672 0:eb94c702a103 38
m193672 0:eb94c702a103 39 // Function definition Prototypes (declarations)
m193672 0:eb94c702a103 40 void ctrCode(); // declare that a separate (other than main) function named "ctrCode" exists
m193672 0:eb94c702a103 41 void twoStepCode();
m193672 0:eb94c702a103 42 void pCtrlCode();
m193672 0:eb94c702a103 43 void PIctrlCode();
m193672 0:eb94c702a103 44
m193672 0:eb94c702a103 45
m193672 0:eb94c702a103 46 // Enter main function
m193672 0:eb94c702a103 47 int main ()
m193672 0:eb94c702a103 48 {
m193672 0:eb94c702a103 49 // Initializes mbed to access functionality of encoder, A/D, driver, etc. chipsets
m193672 0:eb94c702a103 50 // Input is baud rate for PC communication
m193672 0:eb94c702a103 51 mbedWSEsbcInit(115200); // also initializes timer object t
m193672 0:eb94c702a103 52 mot_en1.period(0.020); // sets PWM period to 0.02 seconds for best DC motor operation
m193672 0:eb94c702a103 53
m193672 0:eb94c702a103 54 while(1) {
m193672 0:eb94c702a103 55
m193672 0:eb94c702a103 56 // Scan serial port for user input to begin experiment
m193672 0:eb94c702a103 57 //pc.scanf("%f,%f",&TotalTime,&lowDC);
m193672 0:eb94c702a103 58 pc.scanf("%f,%f,%f",&TotalTime,&dspd,&Kp);
m193672 0:eb94c702a103 59
m193672 0:eb94c702a103 60 // perform necessary functions to time the experiment
m193672 0:eb94c702a103 61 Time = 0.0; // reset time variable
m193672 0:eb94c702a103 62 t.reset(); // reset timer object
m193672 0:eb94c702a103 63
m193672 0:eb94c702a103 64 // Attach the ctrCode function to ticker object specified with period Ts
m193672 0:eb94c702a103 65 //Controller.attach(&twoStepCode,Ts);
m193672 0:eb94c702a103 66 Controller.attach(&PIctrlCode,Ts);
m193672 0:eb94c702a103 67
m193672 0:eb94c702a103 68 t.start(); // start measuring elapsed time
m193672 0:eb94c702a103 69
m193672 0:eb94c702a103 70 // perform operations while the elapsed time is less than the desired total time
m193672 0:eb94c702a103 71 while(Time <= TotalTime) {
m193672 0:eb94c702a103 72
m193672 0:eb94c702a103 73
m193672 0:eb94c702a103 74
m193672 0:eb94c702a103 75 // read current elapsed time
m193672 0:eb94c702a103 76 Time = t.read();
m193672 0:eb94c702a103 77
m193672 0:eb94c702a103 78
m193672 0:eb94c702a103 79 // send data over serial port
m193672 0:eb94c702a103 80 pc.printf("%f,%f,%f\n",Time,speed,dc);
m193672 0:eb94c702a103 81
m193672 0:eb94c702a103 82
m193672 0:eb94c702a103 83 wait(Tstrm); // print data at approximately 50 Hz
m193672 0:eb94c702a103 84
m193672 0:eb94c702a103 85
m193672 0:eb94c702a103 86
m193672 0:eb94c702a103 87
m193672 0:eb94c702a103 88 } // end while(Time<=Ttime)
m193672 0:eb94c702a103 89
m193672 0:eb94c702a103 90 Controller.detach(); // detach ticker to turn off controller
m193672 0:eb94c702a103 91 // Turn motor off at end of experiment
m193672 0:eb94c702a103 92 mot_control(1,0.0);
m193672 0:eb94c702a103 93
m193672 0:eb94c702a103 94 }// end while(1)
m193672 0:eb94c702a103 95 }// end main
m193672 0:eb94c702a103 96
m193672 0:eb94c702a103 97
m193672 0:eb94c702a103 98
m193672 0:eb94c702a103 99 // Additional function definitions
m193672 0:eb94c702a103 100 void ctrCode() // function to attach to ticker
m193672 0:eb94c702a103 101 {
m193672 0:eb94c702a103 102
m193672 0:eb94c702a103 103 myled = !myled; // toggle LED 2 to indicate control update
m193672 0:eb94c702a103 104
m193672 0:eb94c702a103 105 // Read encoder
m193672 0:eb94c702a103 106 enc1 = LS7366_read_counter(1); // input is the encoder channel
m193672 0:eb94c702a103 107
m193672 0:eb94c702a103 108 // Convert from counts to radians
m193672 0:eb94c702a103 109 ang = 2.0*PI*enc1/6400.0;
m193672 0:eb94c702a103 110
m193672 0:eb94c702a103 111 // Estimate speed
m193672 0:eb94c702a103 112 speed = (ang-angp)/Ts;
m193672 0:eb94c702a103 113
m193672 0:eb94c702a103 114 // Age variables
m193672 0:eb94c702a103 115 angp = ang;
m193672 0:eb94c702a103 116
m193672 0:eb94c702a103 117 // compute duty cycle for motor (will be changed later!)
m193672 0:eb94c702a103 118 dc = usrDC; // user-specified duty cycle
m193672 0:eb94c702a103 119
m193672 0:eb94c702a103 120 // motor control
m193672 0:eb94c702a103 121 mot_control(1,dc); // first input is the motor channel, second is duty cycle
m193672 0:eb94c702a103 122
m193672 0:eb94c702a103 123 } // end ctrCode()
m193672 0:eb94c702a103 124
m193672 0:eb94c702a103 125
m193672 0:eb94c702a103 126
m193672 0:eb94c702a103 127
m193672 0:eb94c702a103 128 // Additional function definitions
m193672 0:eb94c702a103 129 void twoStepCode() // function to attach to ticker
m193672 0:eb94c702a103 130 {
m193672 0:eb94c702a103 131 myled = !myled; // toggle LED 2 to indicate control update
m193672 0:eb94c702a103 132
m193672 0:eb94c702a103 133 // Read encoder
m193672 0:eb94c702a103 134 enc1 = LS7366_read_counter(1); // input is the encoder channel
m193672 0:eb94c702a103 135
m193672 0:eb94c702a103 136 // Convert from counts to radians
m193672 0:eb94c702a103 137 ang = 2.0*PI*enc1/6400.0;
m193672 0:eb94c702a103 138
m193672 0:eb94c702a103 139 // Estimate speed
m193672 0:eb94c702a103 140 speed = (ang-angp)/Ts;
m193672 0:eb94c702a103 141
m193672 0:eb94c702a103 142 // Age variables
m193672 0:eb94c702a103 143 angp = ang;
m193672 0:eb94c702a103 144
m193672 0:eb94c702a103 145 // compute duty cycle for motor over two step inputs
m193672 0:eb94c702a103 146 if(Time<0.1) {
m193672 0:eb94c702a103 147 dc = 0.0;
m193672 0:eb94c702a103 148 } else if(Time<0.55) {
m193672 0:eb94c702a103 149 dc = lowDC; // low duty cycle
m193672 0:eb94c702a103 150 } else {
m193672 0:eb94c702a103 151 dc = 0.10;
m193672 0:eb94c702a103 152 }
m193672 0:eb94c702a103 153 // motor control
m193672 0:eb94c702a103 154 mot_control(1,dc); // first input is the motor channel, second is duty cycle
m193672 0:eb94c702a103 155 } // end twoStepCode()
m193672 0:eb94c702a103 156
m193672 0:eb94c702a103 157 void pCtrlCode() // function to attach to ticker
m193672 0:eb94c702a103 158 {
m193672 0:eb94c702a103 159 myled = !myled; // toggle LED 2 to indicate control update
m193672 0:eb94c702a103 160
m193672 0:eb94c702a103 161 // Read encoder
m193672 0:eb94c702a103 162 enc1 = LS7366_read_counter(1); // input is the encoder channel
m193672 0:eb94c702a103 163
m193672 0:eb94c702a103 164 // Convert from counts to radians
m193672 0:eb94c702a103 165 ang = 2.0*PI*enc1/6400.0;
m193672 0:eb94c702a103 166
m193672 0:eb94c702a103 167 // Estimate speed
m193672 0:eb94c702a103 168 speed = (ang-angp)/Ts;
m193672 0:eb94c702a103 169
m193672 0:eb94c702a103 170 // Age variables
m193672 0:eb94c702a103 171 angp = ang;
m193672 0:eb94c702a103 172
m193672 0:eb94c702a103 173 // compute error
m193672 0:eb94c702a103 174 spdErr = dspd-speed;
m193672 0:eb94c702a103 175
m193672 0:eb94c702a103 176 // compute duty cycle for motor
m193672 0:eb94c702a103 177 if(Time<0.1) {
m193672 0:eb94c702a103 178 dc = 0.0;
m193672 0:eb94c702a103 179 } else {
m193672 0:eb94c702a103 180 dc = Kp*(spdErr);
m193672 0:eb94c702a103 181 }
m193672 0:eb94c702a103 182 // enforce duty cycle saturation
m193672 0:eb94c702a103 183 if(dc>1.0) {
m193672 0:eb94c702a103 184 dc = 1.0;
m193672 0:eb94c702a103 185 } else if(dc<-1.0) {
m193672 0:eb94c702a103 186 dc = -1.0;
m193672 0:eb94c702a103 187 }
m193672 0:eb94c702a103 188 // motor control
m193672 0:eb94c702a103 189 mot_control(1,dc); // first input is the motor channel, second is duty cycle
m193672 0:eb94c702a103 190 }
m193672 0:eb94c702a103 191
m193672 0:eb94c702a103 192 void PIctrlCode() // function to attach to ticker
m193672 0:eb94c702a103 193 {
m193672 0:eb94c702a103 194 myled = !myled; // toggle LED 2 to indicate control update
m193672 0:eb94c702a103 195
m193672 0:eb94c702a103 196 // Read encoder
m193672 0:eb94c702a103 197 enc1 = LS7366_read_counter(1); // input is the encoder channel
m193672 0:eb94c702a103 198
m193672 0:eb94c702a103 199 // Convert from counts to radians
m193672 0:eb94c702a103 200 ang = 2.0*PI*enc1/6400.0;
m193672 0:eb94c702a103 201
m193672 0:eb94c702a103 202 // Estimate speed
m193672 0:eb94c702a103 203 speed = (ang-angp)/Ts;
m193672 0:eb94c702a103 204
m193672 0:eb94c702a103 205 // Age variables
m193672 0:eb94c702a103 206 angp = ang;
m193672 0:eb94c702a103 207
m193672 0:eb94c702a103 208 // compute error
m193672 0:eb94c702a103 209 spdErr = dspd-speed;
m193672 0:eb94c702a103 210
m193672 0:eb94c702a103 211
m193672 0:eb94c702a103 212 // compute duty cycle for motor
m193672 0:eb94c702a103 213 if(Time<0.1) {
m193672 0:eb94c702a103 214 dc = 0.0;
m193672 0:eb94c702a103 215 } else {
m193672 0:eb94c702a103 216 dc = dc_p + 0.03554*spdErr - 0.030412*spdErr_p;
m193672 0:eb94c702a103 217 //dc = dc_p+(0.6593/20)*(1.079*spdErr-0.9213*spdErr_p);
m193672 0:eb94c702a103 218 }
m193672 0:eb94c702a103 219 dc_p = dc;
m193672 0:eb94c702a103 220 spdErr_p = spdErr;
m193672 0:eb94c702a103 221 // enforce duty cycle saturation
m193672 0:eb94c702a103 222 if(dc>1.0) {
m193672 0:eb94c702a103 223 dc = 1.0;
m193672 0:eb94c702a103 224 } else if(dc<-1.0) {
m193672 0:eb94c702a103 225 dc = -1.0;
m193672 0:eb94c702a103 226 }
m193672 0:eb94c702a103 227 // motor control
m193672 0:eb94c702a103 228 mot_control(1,dc); // first input is the motor channel, second is duty cycle
m193672 0:eb94c702a103 229 }
m193672 0:eb94c702a103 230
m193672 0:eb94c702a103 231