Bas van Buuren / Mbed 2 deprecated motor_input

Functie van het slaan van de bal

Dependencies:   Encoder HIDScope mbed-dsp mbed MODSERIAL

main.cpp@11:a9f4f296d7ed, 2014-10-29 (annotated)

Committer:
BasvanBuuren
Date:
Wed Oct 29 11:01:55 2014 +0000
Revision:
11:a9f4f296d7ed
Parent:
10:57f60ac40cf6
Child:
12:5ea8de059fa1
$w4g!

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 9:65e998f810d1 30 float new_speed;
BasvanBuuren 9:65e998f810d1 31 float max_speed;
BasvanBuuren 9:65e998f810d1 32 float enca;
BasvanBuuren 0:8af618dc161b 33
BasvanBuuren 5:daf561abb67d 34 int main()
BasvanBuuren 5:daf561abb67d 35 {
BasvanBuuren 5:daf561abb67d 36 while(1)
BasvanBuuren 5:daf561abb67d 37 {
BasvanBuuren 5:daf561abb67d 38 cin >> y1;
BasvanBuuren 5:daf561abb67d 39 cout << y1 << endl;
BasvanBuuren 6:3c4a2afb11e5 40 //slam();
BasvanBuuren 6:3c4a2afb11e5 41 slam_II();
BasvanBuuren 5:daf561abb67d 42 }
BasvanBuuren 5:daf561abb67d 43 }
BasvanBuuren 5:daf561abb67d 44
BasvanBuuren 6:3c4a2afb11e5 45 /*void slam()
BasvanBuuren 0:8af618dc161b 46 {
BasvanBuuren 0:8af618dc161b 47 float enca;
BasvanBuuren 0:8af618dc161b 48 enca=encoderA.getPosition();
BasvanBuuren 0:8af618dc161b 49
BasvanBuuren 3:e0389c90de3d 50 //if (enca <= 0)
BasvanBuuren 0:8af618dc161b 51 {
BasvanBuuren 0:8af618dc161b 52 switch(y1)
BasvanBuuren 0:8af618dc161b 53 {
BasvanBuuren 0:8af618dc161b 54
BasvanBuuren 0:8af618dc161b 55 case 1:
BasvanBuuren 0:8af618dc161b 56 m1_dir=1;
BasvanBuuren 0:8af618dc161b 57 m1_speed=0.33;
BasvanBuuren 1:729c475805f5 58 m1_speedout=m1_speed;
BasvanBuuren 0:8af618dc161b 59 break;
BasvanBuuren 0:8af618dc161b 60 case 2:
BasvanBuuren 0:8af618dc161b 61 m1_dir=1;
BasvanBuuren 0:8af618dc161b 62 m1_speed=0.66;
BasvanBuuren 1:729c475805f5 63 m1_speedout=m1_speed;
BasvanBuuren 0:8af618dc161b 64 break;
BasvanBuuren 0:8af618dc161b 65 case 3:
BasvanBuuren 0:8af618dc161b 66 m1_dir=1;
BasvanBuuren 0:8af618dc161b 67 m1_speed=1.0;
BasvanBuuren 1:729c475805f5 68 m1_speedout=m1_speed;
BasvanBuuren 0:8af618dc161b 69 break;
BasvanBuuren 0:8af618dc161b 70 default :
BasvanBuuren 0:8af618dc161b 71 m1_speed=0;
BasvanBuuren 0:8af618dc161b 72 }
BasvanBuuren 1:729c475805f5 73 while (enca < MAXENCO)
BasvanBuuren 1:729c475805f5 74 {
BasvanBuuren 1:729c475805f5 75 enca=encoderA.getPosition();
BasvanBuuren 1:729c475805f5 76 cout << enca << endl;
BasvanBuuren 1:729c475805f5 77 }
BasvanBuuren 0:8af618dc161b 78 m1_speed=0;
BasvanBuuren 1:729c475805f5 79 m1_speedout=m1_speed;
BasvanBuuren 0:8af618dc161b 80 m1_dir=0;
BasvanBuuren 2:f86efc62a093 81 cout << "blieb!" << endl;
BasvanBuuren 2:f86efc62a093 82 m1_speed=0.4;
BasvanBuuren 2:f86efc62a093 83 m1_speedout=m1_speed;
BasvanBuuren 1:729c475805f5 84 while(enca > MINENCO)
BasvanBuuren 0:8af618dc161b 85 {
BasvanBuuren 1:729c475805f5 86 enca=encoderA.getPosition();
BasvanBuuren 2:f86efc62a093 87 cout << enca << endl;
BasvanBuuren 0:8af618dc161b 88 }
BasvanBuuren 0:8af618dc161b 89 m1_speed=0;
BasvanBuuren 1:729c475805f5 90 m1_speedout=m1_speed;
BasvanBuuren 0:8af618dc161b 91 }
BasvanBuuren 6:3c4a2afb11e5 92 }*/
BasvanBuuren 6:3c4a2afb11e5 93
BasvanBuuren 6:3c4a2afb11e5 94 void slam_II()
BasvanBuuren 6:3c4a2afb11e5 95 {
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 7:36a7a7136be3 102 break;
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 7:36a7a7136be3 107 break;
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 7:36a7a7136be3 112 break;
BasvanBuuren 6:3c4a2afb11e5 113 default:
BasvanBuuren 6:3c4a2afb11e5 114 m1_dir=1;
BasvanBuuren 6:3c4a2afb11e5 115 m1_speed=0;
BasvanBuuren 7:36a7a7136be3 116 break;
BasvanBuuren 6:3c4a2afb11e5 117 }
BasvanBuuren 11:a9f4f296d7ed 118 new_speed=pid(MAXENCO,encoderA.getPosition());
BasvanBuuren 11:a9f4f296d7ed 119 while(new_speed!=0)
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 11:a9f4f296d7ed 134 new_speed=pid(MINENCO,encoderA.getPosition());
BasvanBuuren 11:a9f4f296d7ed 135 while(new_speed!=0)
BasvanBuuren 6:3c4a2afb11e5 136 {
BasvanBuuren 6:3c4a2afb11e5 137 new_speed=pid(MINENCO,encoderA.getPosition());
BasvanBuuren 6:3c4a2afb11e5 138 clamp(&new_speed,-0.5,0.5);
BasvanBuuren 6:3c4a2afb11e5 139 if (new_speed>0)
BasvanBuuren 6:3c4a2afb11e5 140 {
BasvanBuuren 6:3c4a2afb11e5 141 m1_dir=1;
BasvanBuuren 6:3c4a2afb11e5 142 }
BasvanBuuren 6:3c4a2afb11e5 143 else
BasvanBuuren 6:3c4a2afb11e5 144 {
BasvanBuuren 6:3c4a2afb11e5 145 m1_dir=0;
BasvanBuuren 6:3c4a2afb11e5 146 }
BasvanBuuren 6:3c4a2afb11e5 147 m1_speed.write(fabs(new_speed));
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 }