Bas van Buuren / Mbed 2 deprecated motor_input

Functie van het slaan van de bal

Dependencies:   Encoder HIDScope mbed-dsp mbed MODSERIAL

main.cpp@12:5ea8de059fa1, 2014-10-29 (annotated)

Committer:
BasvanBuuren
Date:
Wed Oct 29 13:41:21 2014 +0000
Revision:
12:5ea8de059fa1
Parent:
11:a9f4f296d7ed
Child:
13:b35f3553f210
Beetje schoongemaakt, nog wel wat rare mankementen...

Who changed what in which revision?

UserRevisionLine numberNew contents of line
BasvanBuuren 0:8af618dc161b 1 #include "mbed.h"
BasvanBuuren 0:8af618dc161b 2 #include "HIDScope.h"
BasvanBuuren 0:8af618dc161b 3 //#include "mbed-dsp.h"
BasvanBuuren 0:8af618dc161b 4 #include "encoder.h"
BasvanBuuren 1:729c475805f5 5 #include <iostream>
BasvanBuuren 0:8af618dc161b 6
BasvanBuuren 6:3c4a2afb11e5 7 #define SAMP_TIME 0.01
BasvanBuuren 6:3c4a2afb11e5 8 #define K_P (0.1)
BasvanBuuren 12:5ea8de059fa1 9 #define K_I (0.04 *SAMP_TIME)
BasvanBuuren 12:5ea8de059fa1 10 #define K_D (0.0001 /SAMP_TIME)
BasvanBuuren 5:daf561abb67d 11 #define I_LIMIT 1.
BasvanBuuren 5:daf561abb67d 12
BasvanBuuren 12:5ea8de059fa1 13 #define MAXENCO 256
BasvanBuuren 0:8af618dc161b 14 #define MINENCO 0
BasvanBuuren 0:8af618dc161b 15
BasvanBuuren 5:daf561abb67d 16
BasvanBuuren 5:daf561abb67d 17 //define in and output
BasvanBuuren 0:8af618dc161b 18 Encoder encoderA(PTD0,PTD2);
BasvanBuuren 6:3c4a2afb11e5 19 PwmOut m1_speed(PTA5);
BasvanBuuren 0:8af618dc161b 20 DigitalOut m1_dir(PTA4);
BasvanBuuren 0:8af618dc161b 21
BasvanBuuren 5:daf561abb67d 22 //define functions
BasvanBuuren 12:5ea8de059fa1 23 void slam();
BasvanBuuren 5:daf561abb67d 24 void clamp(float * in, float min, float max);
BasvanBuuren 5:daf561abb67d 25 float pid(float rev_value, float mea_value);
BasvanBuuren 5:daf561abb67d 26
BasvanBuuren 5:daf561abb67d 27 //define global variables
BasvanBuuren 0:8af618dc161b 28 int y1;
BasvanBuuren 9:65e998f810d1 29 float new_speed;
BasvanBuuren 9:65e998f810d1 30 float max_speed;
BasvanBuuren 9:65e998f810d1 31 float enca;
BasvanBuuren 0:8af618dc161b 32
BasvanBuuren 5:daf561abb67d 33 int main()
BasvanBuuren 5:daf561abb67d 34 {
BasvanBuuren 5:daf561abb67d 35 while(1)
BasvanBuuren 5:daf561abb67d 36 {
BasvanBuuren 5:daf561abb67d 37 cin >> y1;
BasvanBuuren 5:daf561abb67d 38 cout << y1 << endl;
BasvanBuuren 12:5ea8de059fa1 39 slam();
BasvanBuuren 5:daf561abb67d 40 }
BasvanBuuren 5:daf561abb67d 41 }
BasvanBuuren 5:daf561abb67d 42
BasvanBuuren 12:5ea8de059fa1 43 void slam()
BasvanBuuren 0:8af618dc161b 44 {
BasvanBuuren 12:5ea8de059fa1 45 cout << "Ik begin met de slafunctie" << endl;
BasvanBuuren 12:5ea8de059fa1 46 cout << encoderA.getPosition() << endl << "check" << endl;
BasvanBuuren 6:3c4a2afb11e5 47 switch (y1)
BasvanBuuren 6:3c4a2afb11e5 48 {
BasvanBuuren 6:3c4a2afb11e5 49 case 1:
BasvanBuuren 12:5ea8de059fa1 50 cout << "zaak1" << endl;
BasvanBuuren 6:3c4a2afb11e5 51 m1_dir=1;
BasvanBuuren 6:3c4a2afb11e5 52 max_speed=0.33;
BasvanBuuren 12:5ea8de059fa1 53 //m1_speed.write(0.33);
BasvanBuuren 12:5ea8de059fa1 54 cout << "Motor 1 draait" << endl;
BasvanBuuren 7:36a7a7136be3 55 break;
BasvanBuuren 6:3c4a2afb11e5 56 case 2:
BasvanBuuren 12:5ea8de059fa1 57 cout << "zaak2"<< endl;
BasvanBuuren 6:3c4a2afb11e5 58 m1_dir=1;
BasvanBuuren 6:3c4a2afb11e5 59 max_speed=0.66;
BasvanBuuren 12:5ea8de059fa1 60 //m1_speed.write(0.66);
BasvanBuuren 12:5ea8de059fa1 61 cout << "Motor 1 draait" << endl;
BasvanBuuren 7:36a7a7136be3 62 break;
BasvanBuuren 6:3c4a2afb11e5 63 case 3:
BasvanBuuren 12:5ea8de059fa1 64 cout << "zaak3"<< endl;
BasvanBuuren 6:3c4a2afb11e5 65 m1_dir=1;
BasvanBuuren 6:3c4a2afb11e5 66 max_speed=1;
BasvanBuuren 12:5ea8de059fa1 67 //m1_speed.write(1);
BasvanBuuren 12:5ea8de059fa1 68 cout << "Motor 1 draait" << endl;
BasvanBuuren 7:36a7a7136be3 69 break;
BasvanBuuren 6:3c4a2afb11e5 70 default:
BasvanBuuren 6:3c4a2afb11e5 71 m1_dir=1;
BasvanBuuren 6:3c4a2afb11e5 72 m1_speed=0;
BasvanBuuren 7:36a7a7136be3 73 break;
BasvanBuuren 6:3c4a2afb11e5 74 }
BasvanBuuren 12:5ea8de059fa1 75 cout << "Ik ben uit de switch" << endl;
BasvanBuuren 11:a9f4f296d7ed 76 new_speed=pid(MAXENCO,encoderA.getPosition());
BasvanBuuren 12:5ea8de059fa1 77 clamp(&new_speed,-max_speed,max_speed);
BasvanBuuren 12:5ea8de059fa1 78 if (new_speed>0)
BasvanBuuren 12:5ea8de059fa1 79 {
BasvanBuuren 12:5ea8de059fa1 80 m1_dir=1;
BasvanBuuren 12:5ea8de059fa1 81 }
BasvanBuuren 12:5ea8de059fa1 82 else
BasvanBuuren 6:3c4a2afb11e5 83 {
BasvanBuuren 12:5ea8de059fa1 84 m1_dir=0;
BasvanBuuren 12:5ea8de059fa1 85 }
BasvanBuuren 12:5ea8de059fa1 86 m1_speed.write(fabs(new_speed));
BasvanBuuren 12:5ea8de059fa1 87
BasvanBuuren 12:5ea8de059fa1 88 while(new_speed<-0.1 || new_speed>0.1)
BasvanBuuren 12:5ea8de059fa1 89 {
BasvanBuuren 12:5ea8de059fa1 90 //cout << "Eat, Sleep, Rave, Repeat" << endl;
BasvanBuuren 6:3c4a2afb11e5 91 new_speed=pid(MAXENCO,encoderA.getPosition());
BasvanBuuren 6:3c4a2afb11e5 92 clamp(&new_speed,-max_speed,max_speed);
BasvanBuuren 6:3c4a2afb11e5 93 if (new_speed>0)
BasvanBuuren 6:3c4a2afb11e5 94 {
BasvanBuuren 6:3c4a2afb11e5 95 m1_dir=1;
BasvanBuuren 6:3c4a2afb11e5 96 }
BasvanBuuren 6:3c4a2afb11e5 97 else
BasvanBuuren 6:3c4a2afb11e5 98 {
BasvanBuuren 6:3c4a2afb11e5 99 m1_dir=0;
BasvanBuuren 6:3c4a2afb11e5 100 }
BasvanBuuren 6:3c4a2afb11e5 101 m1_speed.write(fabs(new_speed));
BasvanBuuren 12:5ea8de059fa1 102 //cout << new_speed << endl;
BasvanBuuren 6:3c4a2afb11e5 103 }
BasvanBuuren 12:5ea8de059fa1 104 cout << "Ik ben uit de eerste whilelus" << endl;
BasvanBuuren 12:5ea8de059fa1 105 cout << encoderA.getPosition() << endl << "check" << endl;
BasvanBuuren 11:a9f4f296d7ed 106 new_speed=pid(MINENCO,encoderA.getPosition());
BasvanBuuren 12:5ea8de059fa1 107 clamp(&new_speed,-max_speed,max_speed);
BasvanBuuren 12:5ea8de059fa1 108 if (new_speed>0)
BasvanBuuren 12:5ea8de059fa1 109 {
BasvanBuuren 12:5ea8de059fa1 110 m1_dir=1;
BasvanBuuren 12:5ea8de059fa1 111 }
BasvanBuuren 12:5ea8de059fa1 112 else
BasvanBuuren 12:5ea8de059fa1 113 {
BasvanBuuren 12:5ea8de059fa1 114 m1_dir=0;
BasvanBuuren 12:5ea8de059fa1 115 }
BasvanBuuren 12:5ea8de059fa1 116 m1_speed.write(fabs(new_speed));
BasvanBuuren 12:5ea8de059fa1 117
BasvanBuuren 12:5ea8de059fa1 118 while(new_speed<-0.1 || new_speed>0.1)
BasvanBuuren 6:3c4a2afb11e5 119 {
BasvanBuuren 6:3c4a2afb11e5 120 new_speed=pid(MINENCO,encoderA.getPosition());
BasvanBuuren 6:3c4a2afb11e5 121 clamp(&new_speed,-0.5,0.5);
BasvanBuuren 6:3c4a2afb11e5 122 if (new_speed>0)
BasvanBuuren 6:3c4a2afb11e5 123 {
BasvanBuuren 6:3c4a2afb11e5 124 m1_dir=1;
BasvanBuuren 6:3c4a2afb11e5 125 }
BasvanBuuren 6:3c4a2afb11e5 126 else
BasvanBuuren 6:3c4a2afb11e5 127 {
BasvanBuuren 6:3c4a2afb11e5 128 m1_dir=0;
BasvanBuuren 6:3c4a2afb11e5 129 }
BasvanBuuren 6:3c4a2afb11e5 130 m1_speed.write(fabs(new_speed));
BasvanBuuren 6:3c4a2afb11e5 131 }
BasvanBuuren 12:5ea8de059fa1 132 m1_speed.write(0);
BasvanBuuren 12:5ea8de059fa1 133 cout << "Ik ben uit de tweede whilelus" << endl;
BasvanBuuren 12:5ea8de059fa1 134 cout << encoderA.getPosition() << endl << "check" << endl;
BasvanBuuren 0:8af618dc161b 135 }
BasvanBuuren 0:8af618dc161b 136
BasvanBuuren 5:daf561abb67d 137 void clamp(float * in, float min, float max)
BasvanBuuren 5:daf561abb67d 138 {
BasvanBuuren 5:daf561abb67d 139 *in > min ? * in < max? : *in = max : *in = min;
BasvanBuuren 5:daf561abb67d 140 }
BasvanBuuren 5:daf561abb67d 141
BasvanBuuren 5:daf561abb67d 142 float pid(float rev_value, float mea_value)
BasvanBuuren 0:8af618dc161b 143 {
BasvanBuuren 5:daf561abb67d 144 float error;
BasvanBuuren 5:daf561abb67d 145 static float prev_error = 0;
BasvanBuuren 5:daf561abb67d 146 float p_out = 0;
BasvanBuuren 5:daf561abb67d 147 static float i_out = 0;
BasvanBuuren 5:daf561abb67d 148 float d_out = 0;
BasvanBuuren 5:daf561abb67d 149 error = rev_value - mea_value;
BasvanBuuren 5:daf561abb67d 150 p_out = error * K_P;
BasvanBuuren 5:daf561abb67d 151 i_out += error * K_I;
BasvanBuuren 5:daf561abb67d 152 d_out = (error - prev_error) * K_D;
BasvanBuuren 5:daf561abb67d 153 clamp(&i_out,-I_LIMIT,I_LIMIT);
BasvanBuuren 5:daf561abb67d 154 prev_error=error;
BasvanBuuren 5:daf561abb67d 155 return p_out + i_out + d_out;
BasvanBuuren 0:8af618dc161b 156 }