Dominique Clevers / Mbed 2 deprecated Project5-Motorcontrol

motor aansturing

Dependencies:   Encoder HIDScope MODSERIAL mbed-dsp mbed

main.cpp@1:82a5126adc56, 2014-10-14 (annotated)

Committer:
DominiqueC
Date:
Tue Oct 14 13:11:03 2014 +0000
Revision:
1:82a5126adc56
Parent:
0:1d02168661ec 
voor Daan

Who changed what in which revision?

UserRevisionLine numberNew contents of line
DominiqueC 0:1d02168661ec 1 /***************************************/
DominiqueC 0:1d02168661ec 2 /* */
DominiqueC 0:1d02168661ec 3 /* BRONCODE GROEP 5, MODULE 9, 2014 */
DominiqueC 0:1d02168661ec 4 /* *****-THE SLAP-****** */
DominiqueC 0:1d02168661ec 5 /* */
DominiqueC 0:1d02168661ec 6 /* -Dominique Clevers */
DominiqueC 0:1d02168661ec 7 /* -Rianne van Dommelen */
DominiqueC 0:1d02168661ec 8 /* -Daan de Muinck Keizer */
DominiqueC 0:1d02168661ec 9 /* -David den Houting */
DominiqueC 0:1d02168661ec 10 /* -Marjolein Thijssen */
DominiqueC 0:1d02168661ec 11 /***************************************/
DominiqueC 0:1d02168661ec 12 #include "mbed.h"
DominiqueC 0:1d02168661ec 13 #include "encoder.h"
DominiqueC 0:1d02168661ec 14 #include "MODSERIAL.h"
DominiqueC 1:82a5126adc56 15 #include "HIDScope.h"
DominiqueC 1:82a5126adc56 16
DominiqueC 1:82a5126adc56 17
DominiqueC 1:82a5126adc56 18 //MAXIMALE UITWIJKING BEPALEN
DominiqueC 1:82a5126adc56 19 /** keep_in_range -> float in, and keep_in_range if less than min, or larger than max **/
DominiqueC 1:82a5126adc56 20 void keep_in_range(float * in, float min, float max);
DominiqueC 0:1d02168661ec 21
DominiqueC 1:82a5126adc56 22 /** variable to show when a new loop can be started*/
DominiqueC 1:82a5126adc56 23 /** volatile means that it can be changed in an */
DominiqueC 1:82a5126adc56 24 /** interrupt routine, and that that change is vis-*/
DominiqueC 1:82a5126adc56 25 /** ible in the main loop. */
DominiqueC 1:82a5126adc56 26
DominiqueC 1:82a5126adc56 27 volatile bool looptimerflag;
DominiqueC 1:82a5126adc56 28
DominiqueC 1:82a5126adc56 29 /** function called by Ticker "looptimer" */
DominiqueC 1:82a5126adc56 30 /** variable 'looptimerflag' is set to 'true' */
DominiqueC 1:82a5126adc56 31 /** each time the looptimer expires. */
DominiqueC 1:82a5126adc56 32 void setlooptimerflag(void)
DominiqueC 0:1d02168661ec 33 {
DominiqueC 1:82a5126adc56 34 looptimerflag = true;
DominiqueC 1:82a5126adc56 35 }
DominiqueC 1:82a5126adc56 36
DominiqueC 1:82a5126adc56 37 int main() {
DominiqueC 1:82a5126adc56 38 //LOCAL VARIABLES
DominiqueC 1:82a5126adc56 39 /*Potmeter input*/
DominiqueC 1:82a5126adc56 40 AnalogIn potmeter(PTC2);
DominiqueC 1:82a5126adc56 41 /* Encoder, using my encoder library */
DominiqueC 1:82a5126adc56 42 /* First pin should be PTDx or PTAx */
DominiqueC 1:82a5126adc56 43 /* because those pins can be used as */
DominiqueC 1:82a5126adc56 44 /* InterruptIn */
DominiqueC 0:1d02168661ec 45 Encoder motor1(PTD0,PTC9);
DominiqueC 1:82a5126adc56 46 /* MODSERIAL to get non-blocking Serial*/
DominiqueC 0:1d02168661ec 47 MODSERIAL pc(USBTX,USBRX);
DominiqueC 1:82a5126adc56 48 /* PWM control to motor */
DominiqueC 1:82a5126adc56 49 PwmOut pwm_motor(PTA12);
DominiqueC 1:82a5126adc56 50 /* Direction pin */
DominiqueC 1:82a5126adc56 51 DigitalOut motordir(PTD3);
DominiqueC 1:82a5126adc56 52 /* variable to store setpoint in */
DominiqueC 1:82a5126adc56 53 float setpoint;
DominiqueC 1:82a5126adc56 54 /* variable to store pwm value in*/
DominiqueC 1:82a5126adc56 55 float pwm_to_motor;
DominiqueC 1:82a5126adc56 56 //START OF CODE
DominiqueC 1:82a5126adc56 57
DominiqueC 1:82a5126adc56 58 /*Set the baudrate (use this number in RealTerm too! */
DominiqueC 1:82a5126adc56 59 pc.baud(230400);
DominiqueC 1:82a5126adc56 60
DominiqueC 1:82a5126adc56 61 /*Create a ticker, and let it call the */
DominiqueC 1:82a5126adc56 62 /*function 'setlooptimerflag' every 0.01s */
DominiqueC 1:82a5126adc56 63 Ticker looptimer;
DominiqueC 1:82a5126adc56 64 looptimer.attach(setlooptimerflag,0.01);
DominiqueC 1:82a5126adc56 65
DominiqueC 1:82a5126adc56 66 //INFINITE LOOP
DominiqueC 1:82a5126adc56 67 while(1) {
DominiqueC 1:82a5126adc56 68 /* Wait until looptimer flag is true. */
DominiqueC 1:82a5126adc56 69 /* '!=' means not-is-equal */
DominiqueC 1:82a5126adc56 70 while(looptimerflag != true);
DominiqueC 1:82a5126adc56 71 /* Clear the looptimerflag, otherwise */
DominiqueC 1:82a5126adc56 72 /* the program would simply continue */
DominiqueC 1:82a5126adc56 73 /* without waitingin the next iteration*/
DominiqueC 1:82a5126adc56 74 looptimerflag = false;
DominiqueC 1:82a5126adc56 75
DominiqueC 1:82a5126adc56 76 /* Read potmeter value, apply some math */
DominiqueC 1:82a5126adc56 77 /* to get useful setpoint value */
DominiqueC 1:82a5126adc56 78 setpoint = (potmeter.read()-0.5)*2000;
DominiqueC 1:82a5126adc56 79
DominiqueC 1:82a5126adc56 80 /* Print setpoint and current position to serial terminal*/
DominiqueC 1:82a5126adc56 81 pc.printf("s: %f, %d \n\r", setpoint, motor1.getPosition());
DominiqueC 1:82a5126adc56 82
DominiqueC 1:82a5126adc56 83 /* This is a P-action! calculate error, multiply with gain, and store in pwm_to_motor */
DominiqueC 1:82a5126adc56 84 pwm_to_motor = (setpoint - motor1.getPosition())*.001;
DominiqueC 1:82a5126adc56 85 /* Coerce pwm value if outside range */
DominiqueC 1:82a5126adc56 86 /* Not strictly needed here, but useful */
DominiqueC 1:82a5126adc56 87 /* if doing other calculations with pwm value */
DominiqueC 1:82a5126adc56 88 keep_in_range(&pwm_to_motor, -1,1);
DominiqueC 1:82a5126adc56 89
DominiqueC 1:82a5126adc56 90 /* Control the motor direction pin. based on */
DominiqueC 1:82a5126adc56 91 /* the sign of your pwm value. If your */
DominiqueC 1:82a5126adc56 92 /* motor keeps spinning when running this code */
DominiqueC 1:82a5126adc56 93 /* you probably need to swap the motor wires, */
DominiqueC 1:82a5126adc56 94 /* or swap the 'write(1)' and 'write(0)' below */
DominiqueC 1:82a5126adc56 95 if(pwm_to_motor > 0)
DominiqueC 1:82a5126adc56 96 motordir.write(1);
DominiqueC 1:82a5126adc56 97 else
DominiqueC 1:82a5126adc56 98 motordir.write(0);
DominiqueC 1:82a5126adc56 99 //WRITE VALUE TO MOTOR
DominiqueC 1:82a5126adc56 100 /* Take the absolute value of the PWM to send */
DominiqueC 1:82a5126adc56 101 /* to the motor. */
DominiqueC 1:82a5126adc56 102 pwm_motor.write(abs(pwm_to_motor));
DominiqueC 0:1d02168661ec 103 }
DominiqueC 0:1d02168661ec 104 }
DominiqueC 0:1d02168661ec 105
DominiqueC 1:82a5126adc56 106
DominiqueC 1:82a5126adc56 107 //coerces value 'in' to min or max when exceeding those values
DominiqueC 1:82a5126adc56 108 //if you'd like to understand the statement below take a google for
DominiqueC 1:82a5126adc56 109 //'ternary operators'.
DominiqueC 1:82a5126adc56 110 void keep_in_range(float * in, float min, float max)
DominiqueC 1:82a5126adc56 111 {
DominiqueC 1:82a5126adc56 112 *in > min ? *in < max? : *in = max: *in = min;
DominiqueC 1:82a5126adc56 113 }
DominiqueC 1:82a5126adc56 114
DominiqueC 1:82a5126adc56 115
DominiqueC 0:1d02168661ec 116 //AANSTUREN MOTOR (POSITIE EN SNELHEID)
DominiqueC 0:1d02168661ec 117 #define TSAMP 0.01
DominiqueC 0:1d02168661ec 118 #define K_P (0.1)
DominiqueC 0:1d02168661ec 119 #define K_I (0.03 *TSAMP)
DominiqueC 0:1d02168661ec 120 #define K_D (0.001 /TSAMP)
DominiqueC 0:1d02168661ec 121 #define I_LIMIT 1.
DominiqueC 0:1d02168661ec 122
DominiqueC 0:1d02168661ec 123 #define M1_PWM PTC8
DominiqueC 0:1d02168661ec 124 #define M1_DIR PTC9
DominiqueC 0:1d02168661ec 125 #define M2_PWM PTA5
DominiqueC 0:1d02168661ec 126 #define M2_DIR PTA4
DominiqueC 0:1d02168661ec 127
DominiqueC 0:1d02168661ec 128 //#define POT_AVG 50
DominiqueC 0:1d02168661ec 129
DominiqueC 0:1d02168661ec 130 void clamp(float * in, float min, float max);
DominiqueC 0:1d02168661ec 131 float pid(float setpoint, float measurement);
DominiqueC 0:1d02168661ec 132 volatile bool looptimerflag;
DominiqueC 0:1d02168661ec 133 //float potsamples[POT_AVG];
DominiqueC 0:1d02168661ec 134 HIDScope scope(6);
DominiqueC 0:1d02168661ec 135
DominiqueC 0:1d02168661ec 136
DominiqueC 0:1d02168661ec 137 void setlooptimerflag(void)
DominiqueC 0:1d02168661ec 138 {
DominiqueC 0:1d02168661ec 139 looptimerflag = true;
DominiqueC 0:1d02168661ec 140 }
DominiqueC 0:1d02168661ec 141
DominiqueC 0:1d02168661ec 142 int main()
DominiqueC 0:1d02168661ec 143 {
DominiqueC 0:1d02168661ec 144 AnalogIn potmeter(PTC2);
DominiqueC 0:1d02168661ec 145 //start Encoder-> first pin should be PTDx or PTAx, second pin doesn't matter
DominiqueC 0:1d02168661ec 146 Encoder motor1(PTD0,PTA13);
DominiqueC 0:1d02168661ec 147 /*PwmOut to motor driver*/
DominiqueC 0:1d02168661ec 148 PwmOut pwm_motor(M2_PWM);
DominiqueC 0:1d02168661ec 149 //10kHz PWM frequency
DominiqueC 0:1d02168661ec 150 pwm_motor.period_us(75);
DominiqueC 0:1d02168661ec 151 DigitalOut motordir(M2_DIR);
DominiqueC 0:1d02168661ec 152 Ticker looptimer;
DominiqueC 0:1d02168661ec 153 looptimer.attach(setlooptimerflag,TSAMP);
DominiqueC 0:1d02168661ec 154 while(1) {
DominiqueC 0:1d02168661ec 155 int16_t setpoint;
DominiqueC 0:1d02168661ec 156 float new_pwm;
DominiqueC 0:1d02168661ec 157 /*wait until timer has elapsed*/
DominiqueC 0:1d02168661ec 158 while(!looptimerflag);
DominiqueC 0:1d02168661ec 159 looptimerflag = false; //clear flag
DominiqueC 0:1d02168661ec 160 /*potmeter value: 0-1*/
DominiqueC 0:1d02168661ec 161 setpoint = (potmeter.read()-.5)*500;
DominiqueC 0:1d02168661ec 162 /*new_pwm = (setpoint - motor1.getPosition())*.001; -> P action*/
DominiqueC 0:1d02168661ec 163 new_pwm = pid(setpoint, motor1.getPosition());
DominiqueC 0:1d02168661ec 164 clamp(&new_pwm, -1,1);
DominiqueC 0:1d02168661ec 165 scope.set(0, setpoint);
DominiqueC 0:1d02168661ec 166 scope.set(4, new_pwm);
DominiqueC 0:1d02168661ec 167 scope.set(5, motor1.getPosition());
DominiqueC 0:1d02168661ec 168 // ch 1, 2 and 3 set in pid controller */
DominiqueC 0:1d02168661ec 169 scope.send();
DominiqueC 0:1d02168661ec 170 if(new_pwm < 0)
DominiqueC 0:1d02168661ec 171 motordir = 0;
DominiqueC 0:1d02168661ec 172 else
DominiqueC 0:1d02168661ec 173 motordir = 1;
DominiqueC 0:1d02168661ec 174 pwm_motor.write(abs(new_pwm));
DominiqueC 0:1d02168661ec 175 }
DominiqueC 0:1d02168661ec 176 }
DominiqueC 0:1d02168661ec 177
DominiqueC 0:1d02168661ec 178
DominiqueC 0:1d02168661ec 179 //clamps value 'in' to min or max when exceeding those values
DominiqueC 0:1d02168661ec 180 //if you'd like to understand the statement below take a google for
DominiqueC 0:1d02168661ec 181 //'ternary operators'.
DominiqueC 0:1d02168661ec 182 void clamp(float * in, float min, float max)
DominiqueC 0:1d02168661ec 183 {
DominiqueC 0:1d02168661ec 184 *in > min ? *in < max? : *in = max: *in = min;
DominiqueC 0:1d02168661ec 185 }
DominiqueC 0:1d02168661ec 186
DominiqueC 0:1d02168661ec 187
DominiqueC 0:1d02168661ec 188 float pid(float setpoint, float measurement)
DominiqueC 0:1d02168661ec 189 {
DominiqueC 0:1d02168661ec 190 float error;
DominiqueC 0:1d02168661ec 191 static float prev_error = 0;
DominiqueC 0:1d02168661ec 192 float out_p = 0;
DominiqueC 0:1d02168661ec 193 static float out_i = 0;
DominiqueC 0:1d02168661ec 194 float out_d = 0;
DominiqueC 0:1d02168661ec 195 error = setpoint-measurement;
DominiqueC 0:1d02168661ec 196 out_p = error*K_P;
DominiqueC 0:1d02168661ec 197 out_i += error*K_I;
DominiqueC 0:1d02168661ec 198 out_d = (error-prev_error)*K_D;
DominiqueC 0:1d02168661ec 199 clamp(&out_i,-I_LIMIT,I_LIMIT);
DominiqueC 0:1d02168661ec 200 prev_error = error;
DominiqueC 0:1d02168661ec 201 scope.set(1,out_p);
DominiqueC 0:1d02168661ec 202 scope.set(2,out_i);
DominiqueC 0:1d02168661ec 203 scope.set(3,out_d);
DominiqueC 0:1d02168661ec 204 return out_p + out_i + out_d;
DominiqueC 0:1d02168661ec 205 }
DominiqueC 0:1d02168661ec 206
DominiqueC 1:82a5126adc56 207
DominiqueC 1:82a5126adc56 208 //POSITIE EN SNELHEID UITLEZEN OP PC
DominiqueC 1:82a5126adc56 209 int main()
DominiqueC 1:82a5126adc56 210 {
DominiqueC 1:82a5126adc56 211 while(1) //Loop
DominiqueC 1:82a5126adc56 212 {
DominiqueC 1:82a5126adc56 213 /** Make encoder object.
DominiqueC 1:82a5126adc56 214 * First pin should be on PTAx or PTDx because those pins can be used as InterruptIn
DominiqueC 1:82a5126adc56 215 * Second pin can be any digital input
DominiqueC 1:82a5126adc56 216 */
DominiqueC 1:82a5126adc56 217 Encoder motor1(PTD0,PTC9);
DominiqueC 1:82a5126adc56 218 /*Use USB serial to send values*/
DominiqueC 1:82a5126adc56 219 MODSERIAL pc(USBTX,USBRX);
DominiqueC 1:82a5126adc56 220 /*Set baud rate to 115200*/
DominiqueC 1:82a5126adc56 221 pc.baud(115200);
DominiqueC 1:82a5126adc56 222 while(1) { //Loop
DominiqueC 1:82a5126adc56 223 /**Wait to prevent buffer overflow, and to keep terminal readable (not scrolling too fast)*/
DominiqueC 1:82a5126adc56 224 wait(0.2);
DominiqueC 1:82a5126adc56 225 /** print position (integer) and speed (float) to the PC*/
DominiqueC 1:82a5126adc56 226 pc.printf("pos: %d, speed %f \r\n",motor1.getPosition(), motor1.getSpeed());
DominiqueC 1:82a5126adc56 227 }
DominiqueC 1:82a5126adc56 228 }