Bas van Buuren / Mbed 2 deprecated motor_input

Functie van het slaan van de bal

Dependencies:   Encoder HIDScope mbed-dsp mbed MODSERIAL

main.cpp@6:3c4a2afb11e5, 2014-10-29 (annotated)

Committer:
BasvanBuuren
Date:
Wed Oct 29 10:36:41 2014 +0000
Revision:
6:3c4a2afb11e5
Parent:
5:daf561abb67d
Child:
7:36a7a7136be3
PID regelaar 'vo!

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 6:3c4a2afb11e5 9 #define K_I (0.03 *SAMP_TIME)
BasvanBuuren 6:3c4a2afb11e5 10 #define K_D (0.0005 /SAMP_TIME)
BasvanBuuren 5:daf561abb67d 11 #define I_LIMIT 1.
BasvanBuuren 5:daf561abb67d 12
BasvanBuuren 1:729c475805f5 13 #define MAXENCO 300
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 6:3c4a2afb11e5 23 //void slam();
BasvanBuuren 6:3c4a2afb11e5 24 void slam_II();
BasvanBuuren 5:daf561abb67d 25 void clamp(float * in, float min, float max);
BasvanBuuren 5:daf561abb67d 26 float pid(float rev_value, float mea_value);
BasvanBuuren 5:daf561abb67d 27
BasvanBuuren 5:daf561abb67d 28 //define global variables
BasvanBuuren 0:8af618dc161b 29 int y1;
BasvanBuuren 0:8af618dc161b 30
BasvanBuuren 5:daf561abb67d 31 int main()
BasvanBuuren 5:daf561abb67d 32 {
BasvanBuuren 5:daf561abb67d 33 while(1)
BasvanBuuren 5:daf561abb67d 34 {
BasvanBuuren 5:daf561abb67d 35 cin >> y1;
BasvanBuuren 5:daf561abb67d 36 cout << y1 << endl;
BasvanBuuren 6:3c4a2afb11e5 37 //slam();
BasvanBuuren 6:3c4a2afb11e5 38 slam_II();
BasvanBuuren 5:daf561abb67d 39 }
BasvanBuuren 5:daf561abb67d 40 }
BasvanBuuren 5:daf561abb67d 41
BasvanBuuren 6:3c4a2afb11e5 42 /*void slam()
BasvanBuuren 0:8af618dc161b 43 {
BasvanBuuren 0:8af618dc161b 44 float enca;
BasvanBuuren 0:8af618dc161b 45 enca=encoderA.getPosition();
BasvanBuuren 0:8af618dc161b 46
BasvanBuuren 3:e0389c90de3d 47 //if (enca <= 0)
BasvanBuuren 0:8af618dc161b 48 {
BasvanBuuren 0:8af618dc161b 49 switch(y1)
BasvanBuuren 0:8af618dc161b 50 {
BasvanBuuren 0:8af618dc161b 51
BasvanBuuren 0:8af618dc161b 52 case 1:
BasvanBuuren 0:8af618dc161b 53 m1_dir=1;
BasvanBuuren 0:8af618dc161b 54 m1_speed=0.33;
BasvanBuuren 1:729c475805f5 55 m1_speedout=m1_speed;
BasvanBuuren 0:8af618dc161b 56 break;
BasvanBuuren 0:8af618dc161b 57 case 2:
BasvanBuuren 0:8af618dc161b 58 m1_dir=1;
BasvanBuuren 0:8af618dc161b 59 m1_speed=0.66;
BasvanBuuren 1:729c475805f5 60 m1_speedout=m1_speed;
BasvanBuuren 0:8af618dc161b 61 break;
BasvanBuuren 0:8af618dc161b 62 case 3:
BasvanBuuren 0:8af618dc161b 63 m1_dir=1;
BasvanBuuren 0:8af618dc161b 64 m1_speed=1.0;
BasvanBuuren 1:729c475805f5 65 m1_speedout=m1_speed;
BasvanBuuren 0:8af618dc161b 66 break;
BasvanBuuren 0:8af618dc161b 67 default :
BasvanBuuren 0:8af618dc161b 68 m1_speed=0;
BasvanBuuren 0:8af618dc161b 69 }
BasvanBuuren 1:729c475805f5 70 while (enca < MAXENCO)
BasvanBuuren 1:729c475805f5 71 {
BasvanBuuren 1:729c475805f5 72 enca=encoderA.getPosition();
BasvanBuuren 1:729c475805f5 73 cout << enca << endl;
BasvanBuuren 1:729c475805f5 74 }
BasvanBuuren 0:8af618dc161b 75 m1_speed=0;
BasvanBuuren 1:729c475805f5 76 m1_speedout=m1_speed;
BasvanBuuren 0:8af618dc161b 77 m1_dir=0;
BasvanBuuren 2:f86efc62a093 78 cout << "blieb!" << endl;
BasvanBuuren 2:f86efc62a093 79 m1_speed=0.4;
BasvanBuuren 2:f86efc62a093 80 m1_speedout=m1_speed;
BasvanBuuren 1:729c475805f5 81 while(enca > MINENCO)
BasvanBuuren 0:8af618dc161b 82 {
BasvanBuuren 1:729c475805f5 83 enca=encoderA.getPosition();
BasvanBuuren 2:f86efc62a093 84 cout << enca << endl;
BasvanBuuren 0:8af618dc161b 85 }
BasvanBuuren 0:8af618dc161b 86 m1_speed=0;
BasvanBuuren 1:729c475805f5 87 m1_speedout=m1_speed;
BasvanBuuren 0:8af618dc161b 88 }
BasvanBuuren 6:3c4a2afb11e5 89 }*/
BasvanBuuren 6:3c4a2afb11e5 90
BasvanBuuren 6:3c4a2afb11e5 91 void slam_II()
BasvanBuuren 6:3c4a2afb11e5 92 {
BasvanBuuren 6:3c4a2afb11e5 93 float new_speed;
BasvanBuuren 6:3c4a2afb11e5 94 float max_speed;
BasvanBuuren 6:3c4a2afb11e5 95 float enca;
BasvanBuuren 6:3c4a2afb11e5 96 switch (y1)
BasvanBuuren 6:3c4a2afb11e5 97 {
BasvanBuuren 6:3c4a2afb11e5 98 case 1:
BasvanBuuren 6:3c4a2afb11e5 99 m1_dir=1;
BasvanBuuren 6:3c4a2afb11e5 100 max_speed=0.33;
BasvanBuuren 6:3c4a2afb11e5 101 m1_speed.write(0.33);
BasvanBuuren 6:3c4a2afb11e5 102 return;
BasvanBuuren 6:3c4a2afb11e5 103 case 2:
BasvanBuuren 6:3c4a2afb11e5 104 m1_dir=1;
BasvanBuuren 6:3c4a2afb11e5 105 max_speed=0.66;
BasvanBuuren 6:3c4a2afb11e5 106 m1_speed.write(0.66);
BasvanBuuren 6:3c4a2afb11e5 107 return;
BasvanBuuren 6:3c4a2afb11e5 108 case 3:
BasvanBuuren 6:3c4a2afb11e5 109 m1_dir=1;
BasvanBuuren 6:3c4a2afb11e5 110 max_speed=1;
BasvanBuuren 6:3c4a2afb11e5 111 m1_speed.write(1);
BasvanBuuren 6:3c4a2afb11e5 112 return;
BasvanBuuren 6:3c4a2afb11e5 113 default:
BasvanBuuren 6:3c4a2afb11e5 114 m1_dir=1;
BasvanBuuren 6:3c4a2afb11e5 115 m1_speed=0;
BasvanBuuren 6:3c4a2afb11e5 116 return;
BasvanBuuren 6:3c4a2afb11e5 117 }
BasvanBuuren 6:3c4a2afb11e5 118 enca=encoderA.getPosition();
BasvanBuuren 6:3c4a2afb11e5 119 while(enca!=MAXENCO)
BasvanBuuren 6:3c4a2afb11e5 120 {
BasvanBuuren 6:3c4a2afb11e5 121 new_speed=pid(MAXENCO,encoderA.getPosition());
BasvanBuuren 6:3c4a2afb11e5 122 clamp(&new_speed,-max_speed,max_speed);
BasvanBuuren 6:3c4a2afb11e5 123 if (new_speed>0)
BasvanBuuren 6:3c4a2afb11e5 124 {
BasvanBuuren 6:3c4a2afb11e5 125 m1_dir=1;
BasvanBuuren 6:3c4a2afb11e5 126 }
BasvanBuuren 6:3c4a2afb11e5 127 else
BasvanBuuren 6:3c4a2afb11e5 128 {
BasvanBuuren 6:3c4a2afb11e5 129 m1_dir=0;
BasvanBuuren 6:3c4a2afb11e5 130 }
BasvanBuuren 6:3c4a2afb11e5 131 m1_speed.write(fabs(new_speed));
BasvanBuuren 6:3c4a2afb11e5 132 enca=encoderA.getPosition();
BasvanBuuren 6:3c4a2afb11e5 133 }
BasvanBuuren 6:3c4a2afb11e5 134 while(enca!=MINENCO)
BasvanBuuren 6:3c4a2afb11e5 135 {
BasvanBuuren 6:3c4a2afb11e5 136 new_speed=pid(MINENCO,encoderA.getPosition());
BasvanBuuren 6:3c4a2afb11e5 137 clamp(&new_speed,-0.5,0.5);
BasvanBuuren 6:3c4a2afb11e5 138 if (new_speed>0)
BasvanBuuren 6:3c4a2afb11e5 139 {
BasvanBuuren 6:3c4a2afb11e5 140 m1_dir=1;
BasvanBuuren 6:3c4a2afb11e5 141 }
BasvanBuuren 6:3c4a2afb11e5 142 else
BasvanBuuren 6:3c4a2afb11e5 143 {
BasvanBuuren 6:3c4a2afb11e5 144 m1_dir=0;
BasvanBuuren 6:3c4a2afb11e5 145 }
BasvanBuuren 6:3c4a2afb11e5 146 m1_speed.write(fabs(new_speed));
BasvanBuuren 6:3c4a2afb11e5 147 enca=encoderA.getPosition();
BasvanBuuren 6:3c4a2afb11e5 148 }
BasvanBuuren 0:8af618dc161b 149 }
BasvanBuuren 0:8af618dc161b 150
BasvanBuuren 5:daf561abb67d 151 void clamp(float * in, float min, float max)
BasvanBuuren 5:daf561abb67d 152 {
BasvanBuuren 5:daf561abb67d 153 *in > min ? * in < max? : *in = max : *in = min;
BasvanBuuren 5:daf561abb67d 154 }
BasvanBuuren 5:daf561abb67d 155
BasvanBuuren 5:daf561abb67d 156 float pid(float rev_value, float mea_value)
BasvanBuuren 0:8af618dc161b 157 {
BasvanBuuren 5:daf561abb67d 158 float error;
BasvanBuuren 5:daf561abb67d 159 static float prev_error = 0;
BasvanBuuren 5:daf561abb67d 160 float p_out = 0;
BasvanBuuren 5:daf561abb67d 161 static float i_out = 0;
BasvanBuuren 5:daf561abb67d 162 float d_out = 0;
BasvanBuuren 5:daf561abb67d 163 error = rev_value - mea_value;
BasvanBuuren 5:daf561abb67d 164 p_out = error * K_P;
BasvanBuuren 5:daf561abb67d 165 i_out += error * K_I;
BasvanBuuren 5:daf561abb67d 166 d_out = (error - prev_error) * K_D;
BasvanBuuren 5:daf561abb67d 167 clamp(&i_out,-I_LIMIT,I_LIMIT);
BasvanBuuren 5:daf561abb67d 168 prev_error=error;
BasvanBuuren 5:daf561abb67d 169 return p_out + i_out + d_out;
BasvanBuuren 0:8af618dc161b 170 }