Marijke Abma / Mbed 2 deprecated motor1aansturing201014

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependencies:   Encoder MODSERIAL mbed-dsp mbed

Fork of motor1aansturing by BMT M9 Groep01

main.cpp@15:3a6074f3ceaf, 2014-10-08 (annotated)

Committer:
Jolein
Date:
Wed Oct 08 13:40:40 2014 +0000
Revision:
15:3a6074f3ceaf
Parent:
14:6ec84bfda0f3
Child:
16:bf4c5affd3e9
...;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Jolein 0:eb00992c1597 1 #include "mbed.h"
Jolein 0:eb00992c1597 2 #include "encoder.h"
Jolein 15:3a6074f3ceaf 3 #include "MODSERIAL.h"
Jolein 15:3a6074f3ceaf 4
Jolein 0:eb00992c1597 5 #define TSAMP 0.01
Jolein 0:eb00992c1597 6 #define K_P (0.01)
Jolein 0:eb00992c1597 7 #define K_I (0 *TSAMP)
Jolein 2:ef0fa691e77e 8 #define K_D (0 /TSAMP)
Jolein 0:eb00992c1597 9 #define I_LIMIT 1.
Jolein 0:eb00992c1597 10 #define POT_AVG 50
Jolein 15:3a6074f3ceaf 11 #define WACHT 1
Jolein 15:3a6074f3ceaf 12 #define SLAAN 2
Jolein 15:3a6074f3ceaf 13 #define TERUGKEREN 3
Jolein 0:eb00992c1597 14
Jolein 15:3a6074f3ceaf 15 MODSERIAL pc(USBTX,USBRX);
Jolein 0:eb00992c1597 16 void clamp(float * in, float min, float max);
Jolein 0:eb00992c1597 17 float pid(float setpoint, float measurement);
Jolein 2:ef0fa691e77e 18 //-------------------------------------------------------------------------------------------Input potentiometer-------------------------------
Jolein 0:eb00992c1597 19 AnalogIn potmeter(PTC2);
Jolein 0:eb00992c1597 20 volatile bool looptimerflag;
Jolein 0:eb00992c1597 21 float potsamples[POT_AVG];
Jolein 0:eb00992c1597 22
Jolein 0:eb00992c1597 23
Jolein 0:eb00992c1597 24 void setlooptimerflag(void)
Jolein 0:eb00992c1597 25 {
Jolein 0:eb00992c1597 26 looptimerflag = true;
Jolein 0:eb00992c1597 27 }
Jolein 0:eb00992c1597 28
Jolein 0:eb00992c1597 29 int main()
Jolein 0:eb00992c1597 30 {
Jolein 15:3a6074f3ceaf 31 int toestand = TERUGKEREN;
Jolein 15:3a6074f3ceaf 32 //----------------------------------------------------------------dit is de initiële toestand zodat hij begint met hoek 0--------------------------
Jolein 15:3a6074f3ceaf 33 float angle,v3=0.2,v2=0.1,v1=0.05,anglemax=1
Jolein 0:eb00992c1597 34 Encoder motor1(PTD0,PTC9);
Jolein 0:eb00992c1597 35 PwmOut pwm_motor(PTA5);
Jolein 0:eb00992c1597 36 pwm_motor.period_us(100);
Jolein 0:eb00992c1597 37 DigitalOut motordir(PTD1);
Jolein 0:eb00992c1597 38 Ticker looptimer;
Jolein 0:eb00992c1597 39 looptimer.attach(setlooptimerflag,TSAMP);
Jolein 0:eb00992c1597 40 while(1) {
Jolein 0:eb00992c1597 41 float setpoint;
Jolein 0:eb00992c1597 42 float new_pwm;
Jolein 0:eb00992c1597 43 while(!looptimerflag);
Jolein 2:ef0fa691e77e 44 looptimerflag = false;
Jolein 12:92a64fe08de8 45 //---------------------------------------------------------leest potentiometer (?), probably won't do what I want it to do ------------------
Jolein 15:3a6074f3ceaf 46 //angle = ???? Encoder
Jolein 15:3a6074f3ceaf 47 if(toestand==SLAAN)
Jolein 4:2a3b8c562f4a 48 {
Jolein 15:3a6074f3ceaf 49 if ( 0.75 < potmeter.read() && angle<anglemax) //angle<anglemax moet ergens anders staan (iets met toestand)
Jolein 14:6ec84bfda0f3 50 {
Jolein 14:6ec84bfda0f3 51 setpoint = v3; //experimenteel bepaald, snelheid om bovenste goal te raken
Jolein 14:6ec84bfda0f3 52 }
Jolein 15:3a6074f3ceaf 53 else if ( 0.50 < potmeter.read())
Jolein 14:6ec84bfda0f3 54 {
Jolein 14:6ec84bfda0f3 55 setpoint = v2; //experimenteel bepaald, snelheid om middelste goal te raken
Jolein 14:6ec84bfda0f3 56 }
Jolein 15:3a6074f3ceaf 57 else if (0.25 < potmeter.read())
Jolein 14:6ec84bfda0f3 58 {
Jolein 14:6ec84bfda0f3 59 setpoint = v1; //experimenteel bepaald, snelheid om bovenste goal te raken
Jolein 14:6ec84bfda0f3 60 }
Jolein 14:6ec84bfda0f3 61 else
Jolein 14:6ec84bfda0f3 62 {
Jolein 15:3a6074f3ceaf 63 setpoint = 0; // toestand = TERGUGKEREN?
Jolein 14:6ec84bfda0f3 64 }
Jolein 4:2a3b8c562f4a 65 }
Jolein 15:3a6074f3ceaf 66 //----------------------------------------------------------------------new_pwm = getal----------------------------------------------------
Jolein 0:eb00992c1597 67 new_pwm = pid(setpoint, motor1.getPosition());
Jolein 0:eb00992c1597 68 clamp(&new_pwm, -1,1);
Jolein 4:2a3b8c562f4a 69 //-------------------------------------------------------------------------------------output motor richting-------------------------------------
Jolein 0:eb00992c1597 70 if(new_pwm > 0)
Jolein 0:eb00992c1597 71 motordir = 0;
Jolein 0:eb00992c1597 72 else
Jolein 0:eb00992c1597 73 motordir = 1;
Jolein 0:eb00992c1597 74 pwm_motor.write(abs(new_pwm));
Jolein 15:3a6074f3ceaf 75 if(angle>anglemax && toestand=SLAAN)
Jolein 14:6ec84bfda0f3 76 {
Jolein 15:3a6074f3ceaf 77 toestand = TERUGKEREN;
Jolein 15:3a6074f3ceaf 78 pc.printf ("Toestand = terugkeren!");
Jolein 14:6ec84bfda0f3 79 }
Jolein 0:eb00992c1597 80 }
Jolein 14:6ec84bfda0f3 81
Jolein 0:eb00992c1597 82 }
Jolein 0:eb00992c1597 83
Jolein 0:eb00992c1597 84 void clamp(float * in, float min, float max)
Jolein 0:eb00992c1597 85 {
Jolein 0:eb00992c1597 86 *in > min ? *in < max? : *in = max: *in = min;
Jolein 0:eb00992c1597 87 }
Jolein 0:eb00992c1597 88
Jolein 0:eb00992c1597 89
Jolein 0:eb00992c1597 90 float pid(float setpoint, float measurement)
Jolein 0:eb00992c1597 91 {
Jolein 12:92a64fe08de8 92 //What I want: if getposition gives value beneath min v=0!, If above max: return to 0 position(from the way it came)
Jolein 0:eb00992c1597 93 float error;
Jolein 0:eb00992c1597 94 static float prev_error = 0;
Jolein 0:eb00992c1597 95 float out_p = 0;
Jolein 0:eb00992c1597 96 static float out_i = 0;
Jolein 0:eb00992c1597 97 float out_d = 0;
Jolein 0:eb00992c1597 98 error = setpoint-measurement;
Jolein 0:eb00992c1597 99 out_p = error*K_P;
Jolein 0:eb00992c1597 100 out_i += error*K_I;
Jolein 0:eb00992c1597 101 out_d = (error-prev_error)*K_D;
Jolein 0:eb00992c1597 102 clamp(&out_i,-I_LIMIT,I_LIMIT);
Jolein 0:eb00992c1597 103 prev_error = error;
Jolein 0:eb00992c1597 104 return out_p + out_i + out_d;
Jolein 0:eb00992c1597 105 }