Dominique Clevers / Mbed 2 deprecated Project5-Motorcontrol

motor aansturing

Dependencies:   Encoder HIDScope MODSERIAL mbed-dsp mbed

main.cpp@0:1d02168661ec, 2014-10-13 (annotated)

Committer:
DominiqueC
Date:
Mon Oct 13 08:53:22 2014 +0000
Revision:
0:1d02168661ec
Child:
1:82a5126adc56
motor aansturing; uitlezen computer en snelheid + positie regelen

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 0:1d02168661ec 15 #include "HIDscope.h"
DominiqueC 0:1d02168661ec 16
DominiqueC 0:1d02168661ec 17 //POSITIE EN SNELHEID UITLEZEN OP PC
DominiqueC 0:1d02168661ec 18 int main()
DominiqueC 0:1d02168661ec 19 {
DominiqueC 0:1d02168661ec 20 while(1) //Loop
DominiqueC 0:1d02168661ec 21 {
DominiqueC 0:1d02168661ec 22 /** Make encoder object.
DominiqueC 0:1d02168661ec 23 * First pin should be on PTAx or PTDx because those pins can be used as InterruptIn
DominiqueC 0:1d02168661ec 24 * Second pin can be any digital input
DominiqueC 0:1d02168661ec 25 */
DominiqueC 0:1d02168661ec 26 Encoder motor1(PTD0,PTC9);
DominiqueC 0:1d02168661ec 27 /*Use USB serial to send values*/
DominiqueC 0:1d02168661ec 28 MODSERIAL pc(USBTX,USBRX);
DominiqueC 0:1d02168661ec 29 /*Set baud rate to 115200*/
DominiqueC 0:1d02168661ec 30 pc.baud(115200);
DominiqueC 0:1d02168661ec 31 while(1) { //Loop
DominiqueC 0:1d02168661ec 32 /**Wait to prevent buffer overflow, and to keep terminal readable (not scrolling too fast)*/
DominiqueC 0:1d02168661ec 33 wait(0.2);
DominiqueC 0:1d02168661ec 34 /** print position (integer) and speed (float) to the PC*/
DominiqueC 0:1d02168661ec 35 pc.printf("pos: %d, speed %f \r\n",motor1.getPosition(), motor1.getSpeed());
DominiqueC 0:1d02168661ec 36 }
DominiqueC 0:1d02168661ec 37 }
DominiqueC 0:1d02168661ec 38
DominiqueC 0:1d02168661ec 39 //AANSTUREN MOTOR (POSITIE EN SNELHEID)
DominiqueC 0:1d02168661ec 40 #define TSAMP 0.01
DominiqueC 0:1d02168661ec 41 #define K_P (0.1)
DominiqueC 0:1d02168661ec 42 #define K_I (0.03 *TSAMP)
DominiqueC 0:1d02168661ec 43 #define K_D (0.001 /TSAMP)
DominiqueC 0:1d02168661ec 44 #define I_LIMIT 1.
DominiqueC 0:1d02168661ec 45
DominiqueC 0:1d02168661ec 46 #define M1_PWM PTC8
DominiqueC 0:1d02168661ec 47 #define M1_DIR PTC9
DominiqueC 0:1d02168661ec 48 #define M2_PWM PTA5
DominiqueC 0:1d02168661ec 49 #define M2_DIR PTA4
DominiqueC 0:1d02168661ec 50
DominiqueC 0:1d02168661ec 51 //#define POT_AVG 50
DominiqueC 0:1d02168661ec 52
DominiqueC 0:1d02168661ec 53 void clamp(float * in, float min, float max);
DominiqueC 0:1d02168661ec 54 float pid(float setpoint, float measurement);
DominiqueC 0:1d02168661ec 55 volatile bool looptimerflag;
DominiqueC 0:1d02168661ec 56 //float potsamples[POT_AVG];
DominiqueC 0:1d02168661ec 57 HIDScope scope(6);
DominiqueC 0:1d02168661ec 58
DominiqueC 0:1d02168661ec 59
DominiqueC 0:1d02168661ec 60 void setlooptimerflag(void)
DominiqueC 0:1d02168661ec 61 {
DominiqueC 0:1d02168661ec 62 looptimerflag = true;
DominiqueC 0:1d02168661ec 63 }
DominiqueC 0:1d02168661ec 64
DominiqueC 0:1d02168661ec 65 int main()
DominiqueC 0:1d02168661ec 66 {
DominiqueC 0:1d02168661ec 67 AnalogIn potmeter(PTC2);
DominiqueC 0:1d02168661ec 68 //start Encoder-> first pin should be PTDx or PTAx, second pin doesn't matter
DominiqueC 0:1d02168661ec 69 Encoder motor1(PTD0,PTA13);
DominiqueC 0:1d02168661ec 70 /*PwmOut to motor driver*/
DominiqueC 0:1d02168661ec 71 PwmOut pwm_motor(M2_PWM);
DominiqueC 0:1d02168661ec 72 //10kHz PWM frequency
DominiqueC 0:1d02168661ec 73 pwm_motor.period_us(75);
DominiqueC 0:1d02168661ec 74 DigitalOut motordir(M2_DIR);
DominiqueC 0:1d02168661ec 75 Ticker looptimer;
DominiqueC 0:1d02168661ec 76 looptimer.attach(setlooptimerflag,TSAMP);
DominiqueC 0:1d02168661ec 77 while(1) {
DominiqueC 0:1d02168661ec 78 int16_t setpoint;
DominiqueC 0:1d02168661ec 79 float new_pwm;
DominiqueC 0:1d02168661ec 80 /*wait until timer has elapsed*/
DominiqueC 0:1d02168661ec 81 while(!looptimerflag);
DominiqueC 0:1d02168661ec 82 looptimerflag = false; //clear flag
DominiqueC 0:1d02168661ec 83 /*potmeter value: 0-1*/
DominiqueC 0:1d02168661ec 84 setpoint = (potmeter.read()-.5)*500;
DominiqueC 0:1d02168661ec 85 /*new_pwm = (setpoint - motor1.getPosition())*.001; -> P action*/
DominiqueC 0:1d02168661ec 86 new_pwm = pid(setpoint, motor1.getPosition());
DominiqueC 0:1d02168661ec 87 clamp(&new_pwm, -1,1);
DominiqueC 0:1d02168661ec 88 scope.set(0, setpoint);
DominiqueC 0:1d02168661ec 89 scope.set(4, new_pwm);
DominiqueC 0:1d02168661ec 90 scope.set(5, motor1.getPosition());
DominiqueC 0:1d02168661ec 91 // ch 1, 2 and 3 set in pid controller */
DominiqueC 0:1d02168661ec 92 scope.send();
DominiqueC 0:1d02168661ec 93 if(new_pwm < 0)
DominiqueC 0:1d02168661ec 94 motordir = 0;
DominiqueC 0:1d02168661ec 95 else
DominiqueC 0:1d02168661ec 96 motordir = 1;
DominiqueC 0:1d02168661ec 97 pwm_motor.write(abs(new_pwm));
DominiqueC 0:1d02168661ec 98 }
DominiqueC 0:1d02168661ec 99 }
DominiqueC 0:1d02168661ec 100
DominiqueC 0:1d02168661ec 101
DominiqueC 0:1d02168661ec 102 //clamps value 'in' to min or max when exceeding those values
DominiqueC 0:1d02168661ec 103 //if you'd like to understand the statement below take a google for
DominiqueC 0:1d02168661ec 104 //'ternary operators'.
DominiqueC 0:1d02168661ec 105 void clamp(float * in, float min, float max)
DominiqueC 0:1d02168661ec 106 {
DominiqueC 0:1d02168661ec 107 *in > min ? *in < max? : *in = max: *in = min;
DominiqueC 0:1d02168661ec 108 }
DominiqueC 0:1d02168661ec 109
DominiqueC 0:1d02168661ec 110
DominiqueC 0:1d02168661ec 111 float pid(float setpoint, float measurement)
DominiqueC 0:1d02168661ec 112 {
DominiqueC 0:1d02168661ec 113 float error;
DominiqueC 0:1d02168661ec 114 static float prev_error = 0;
DominiqueC 0:1d02168661ec 115 float out_p = 0;
DominiqueC 0:1d02168661ec 116 static float out_i = 0;
DominiqueC 0:1d02168661ec 117 float out_d = 0;
DominiqueC 0:1d02168661ec 118 error = setpoint-measurement;
DominiqueC 0:1d02168661ec 119 out_p = error*K_P;
DominiqueC 0:1d02168661ec 120 out_i += error*K_I;
DominiqueC 0:1d02168661ec 121 out_d = (error-prev_error)*K_D;
DominiqueC 0:1d02168661ec 122 clamp(&out_i,-I_LIMIT,I_LIMIT);
DominiqueC 0:1d02168661ec 123 prev_error = error;
DominiqueC 0:1d02168661ec 124 scope.set(1,out_p);
DominiqueC 0:1d02168661ec 125 scope.set(2,out_i);
DominiqueC 0:1d02168661ec 126 scope.set(3,out_d);
DominiqueC 0:1d02168661ec 127 return out_p + out_i + out_d;
DominiqueC 0:1d02168661ec 128 }
DominiqueC 0:1d02168661ec 129