Jesse Kaiser / Mbed 2 deprecated Motorcontrol_Gr6

Script voor aansturen motoren

Dependencies:   Encoder HIDScope MODSERIAL mbed-dsp mbed

main.cpp@27:77bb255753be, 2014-10-31 (annotated)

Committer:
jessekaiser
Date:
Fri Oct 31 13:04:51 2014 +0000
Revision:
27:77bb255753be
Parent:
26:5981d4b1144e 
werkt niet meer helemaal

Who changed what in which revision?

UserRevisionLine numberNew contents of line
jessekaiser 0:d5d3b731340c 1 #include "mbed.h"
jessekaiser 0:d5d3b731340c 2 #include "encoder.h"
jessekaiser 0:d5d3b731340c 3 #include "HIDScope.h"
jessekaiser 19:3e46c457091c 4 #include "PwmOut.h"
jessekaiser 0:d5d3b731340c 5
jessekaiser 6:f260176f704b 6 #define TSAMP 0.005
jessekaiser 25:f45d61cc6dc6 7 #define K_P1 (3.5) //voor motor1 is het 3.5
jessekaiser 25:f45d61cc6dc6 8 #define K_I1 (0.01 *TSAMP) //voor motor1 is het 0.01
jessekaiser 27:77bb255753be 9 #define K_P2 (1.0) //3.5
jessekaiser 27:77bb255753be 10 #define K_I2 (0.01 *TSAMP) //0.1
jessekaiser 0:d5d3b731340c 11 #define I_LIMIT 1.
jessekaiser 18:fb85c58a4106 12 #define PI 3.14159265
jessekaiser 18:fb85c58a4106 13 #define l_arm 0.5
jessekaiser 0:d5d3b731340c 14
jessekaiser 7:697293226e5e 15 #define M1_PWM PTC8
jessekaiser 7:697293226e5e 16 #define M1_DIR PTC9
jessekaiser 7:697293226e5e 17 #define M2_PWM PTA5
jessekaiser 7:697293226e5e 18 #define M2_DIR PTA4
jessekaiser 7:697293226e5e 19
jessekaiser 7:697293226e5e 20 //Groene kabel moet op de GROUND en blauw op de 3.3v aansluiting
jessekaiser 0:d5d3b731340c 21
jessekaiser 3:dcc4cebba0d7 22 Serial pc(USBTX, USBRX);
jessekaiser 4:db3dad7e53e3 23 DigitalOut led1(LED_RED);
jessekaiser 5:e4b9cc904928 24 HIDScope scope(2);
jessekaiser 0:d5d3b731340c 25
jessekaiser 7:697293226e5e 26 //motor 25D
jessekaiser 7:697293226e5e 27 Encoder motor1(PTD3,PTD5); //wit, geel
jessekaiser 7:697293226e5e 28 PwmOut pwm_motor1(M2_PWM);
jessekaiser 7:697293226e5e 29 DigitalOut motordir1(M2_DIR);
jessekaiser 3:dcc4cebba0d7 30
jessekaiser 7:697293226e5e 31 //motor2 37D
jessekaiser 7:697293226e5e 32 Encoder motor2(PTD2, PTD0); //wit, geel
jessekaiser 7:697293226e5e 33 PwmOut pwm_motor2(M1_PWM);
jessekaiser 7:697293226e5e 34 DigitalOut motordir2(M1_DIR);
jessekaiser 25:f45d61cc6dc6 35 //void clamp(float * in, float min, float max);
jessekaiser 25:f45d61cc6dc6 36 //float pid(float setpoint, float measurement);
jessekaiser 18:fb85c58a4106 37 float pwm1_percentage = 0;
jessekaiser 18:fb85c58a4106 38 float pwm2_percentage = 0;
jessekaiser 18:fb85c58a4106 39 int cur_pos_motor1;
jessekaiser 18:fb85c58a4106 40 int prev_pos_motor1 = 0;
jessekaiser 18:fb85c58a4106 41 int cur_pos_motor2;
jessekaiser 18:fb85c58a4106 42 int prev_pos_motor2 = 0;
jessekaiser 18:fb85c58a4106 43 float speed1_rad;
jessekaiser 19:3e46c457091c 44 float speed2_rad;
jessekaiser 19:3e46c457091c 45 float pos_motor1_rad;
jessekaiser 19:3e46c457091c 46 float pos_motor2_rad;
jessekaiser 27:77bb255753be 47 int staat1 = 0;
jessekaiser 18:fb85c58a4106 48
jessekaiser 18:fb85c58a4106 49
jessekaiser 7:697293226e5e 50
jessekaiser 21:c856b7752d7d 51
jessekaiser 21:c856b7752d7d 52 void clamp(float* in, float min, float max) // "*" is een pointer (verwijst naar het adres waar een variabele instaat). Dus je slaat niet de variabele op
jessekaiser 21:c856b7752d7d 53 // maar de locatie van de variabele.
jessekaiser 21:c856b7752d7d 54 {
jessekaiser 21:c856b7752d7d 55 *in > min ? /*(*/*in < max? /*niets doen*/ : *in = max/*)*/: *in = min; // a ? b : c --> als a waar is, dan doe je b, en anders c
jessekaiser 21:c856b7752d7d 56 // *in = het getal dat staat op locatie van in --> waarde van new_pwm
jessekaiser 7:697293226e5e 57 }
jessekaiser 3:dcc4cebba0d7 58
jessekaiser 3:dcc4cebba0d7 59
jessekaiser 25:f45d61cc6dc6 60 float pid1(float setpoint1, float measurement1)
jessekaiser 21:c856b7752d7d 61 {
jessekaiser 25:f45d61cc6dc6 62 float error1;
jessekaiser 25:f45d61cc6dc6 63 float out_p1 = 0;
jessekaiser 25:f45d61cc6dc6 64 static float out_i1 = 0;
jessekaiser 25:f45d61cc6dc6 65 error1 = (setpoint1 - measurement1);
jessekaiser 25:f45d61cc6dc6 66 out_p1 = error1*K_P1;
jessekaiser 25:f45d61cc6dc6 67 out_i1 += error1*K_I1;
jessekaiser 25:f45d61cc6dc6 68 clamp(&out_i1,-I_LIMIT,I_LIMIT);
jessekaiser 25:f45d61cc6dc6 69 return out_p1 + out_i1;
jessekaiser 24:7fbd904d191c 70 }
jessekaiser 24:7fbd904d191c 71
jessekaiser 27:77bb255753be 72 float pid2(float setpoint2, float measurement2)
jessekaiser 27:77bb255753be 73 {
jessekaiser 27:77bb255753be 74 float error2;
jessekaiser 27:77bb255753be 75 float out_p2 = 0;
jessekaiser 27:77bb255753be 76 static float out_i2 = 0;
jessekaiser 27:77bb255753be 77 error2 = (setpoint2 - measurement2);
jessekaiser 27:77bb255753be 78 out_p2 = error2*K_P1;
jessekaiser 27:77bb255753be 79 out_i2 += error2*K_I1;
jessekaiser 27:77bb255753be 80 clamp(&out_i2,-I_LIMIT,I_LIMIT);
jessekaiser 27:77bb255753be 81 return out_p2 + out_i2;
jessekaiser 27:77bb255753be 82 }
jessekaiser 27:77bb255753be 83
jessekaiser 25:f45d61cc6dc6 84 float prev_setpoint1 = 0;
jessekaiser 25:f45d61cc6dc6 85 float setpoint1 = 0;
jessekaiser 25:f45d61cc6dc6 86 float prev_setpoint2 = 0;
jessekaiser 25:f45d61cc6dc6 87 float setpoint2 = 0;
jessekaiser 25:f45d61cc6dc6 88
jessekaiser 25:f45d61cc6dc6 89 void batje_links ()
jessekaiser 24:7fbd904d191c 90 {
jessekaiser 27:77bb255753be 91
jessekaiser 27:77bb255753be 92 speed1_rad = 6.0; //positief is CCW, negatief CW (boven aanzicht)
jessekaiser 25:f45d61cc6dc6 93 setpoint1 = prev_setpoint1 + TSAMP * speed1_rad;
jessekaiser 27:77bb255753be 94 if(setpoint1 > (155.0*2.0*PI/360)) { //setpoint in graden
jessekaiser 27:77bb255753be 95 setpoint1 = (155.0*2.0*PI/360);
jessekaiser 25:f45d61cc6dc6 96 }
jessekaiser 27:77bb255753be 97 if(setpoint1 < -(155.0*2.0*PI/360)) {
jessekaiser 27:77bb255753be 98 setpoint1 = -(155.0*2.0*PI/360);
jessekaiser 25:f45d61cc6dc6 99 }
jessekaiser 25:f45d61cc6dc6 100 prev_setpoint1 = setpoint1;
jessekaiser 24:7fbd904d191c 101 }
jessekaiser 24:7fbd904d191c 102
jessekaiser 24:7fbd904d191c 103 void batje_rechts ()
jessekaiser 24:7fbd904d191c 104 {
jessekaiser 27:77bb255753be 105 speed1_rad = 6.0; //positief is CCW, negatief CW (boven aanzicht)
jessekaiser 25:f45d61cc6dc6 106 setpoint1 = prev_setpoint1 + TSAMP * speed1_rad;
jessekaiser 27:77bb255753be 107 if(setpoint1 > (155.0*2.0*PI/360)) { //setpoint in graden
jessekaiser 27:77bb255753be 108 setpoint1 = (155.0*2.0*PI/360);
jessekaiser 25:f45d61cc6dc6 109 }
jessekaiser 27:77bb255753be 110 if(setpoint1 < -(155.0*2.0*PI/360)) {
jessekaiser 27:77bb255753be 111 setpoint1 = -(155.0*2.0*PI/360);
jessekaiser 25:f45d61cc6dc6 112 }
jessekaiser 25:f45d61cc6dc6 113 prev_setpoint1 = setpoint1;
jessekaiser 25:f45d61cc6dc6 114 }
jessekaiser 25:f45d61cc6dc6 115
jessekaiser 25:f45d61cc6dc6 116 void arm_hoog () //LET OP, PAS VARIABELE NOG AAN DIT IS VOOR TESTEN
jessekaiser 25:f45d61cc6dc6 117 {
jessekaiser 25:f45d61cc6dc6 118 speed1_rad = 6.0; //positief is CCW, negatief CW (boven aanzicht)
jessekaiser 25:f45d61cc6dc6 119 setpoint1 = prev_setpoint1 + TSAMP * speed1_rad;
jessekaiser 25:f45d61cc6dc6 120 if(setpoint1 > (155.0*2.0*PI/360)) { //setpoint in graden
jessekaiser 25:f45d61cc6dc6 121 setpoint1 = (155.0*2.0*PI/360);
jessekaiser 25:f45d61cc6dc6 122 }
jessekaiser 25:f45d61cc6dc6 123 if(setpoint1 < -(155.0*2.0*PI/360)) {
jessekaiser 25:f45d61cc6dc6 124 setpoint1 = -(155.0*2.0*PI/360);
jessekaiser 25:f45d61cc6dc6 125 }
jessekaiser 25:f45d61cc6dc6 126 prev_setpoint1 = setpoint1;
jessekaiser 26:5981d4b1144e 127 if(setpoint1 >= (155.0*2.0*PI/360)-0.1) {
jessekaiser 27:77bb255753be 128 staat1 = 1;
jessekaiser 26:5981d4b1144e 129 }
jessekaiser 25:f45d61cc6dc6 130 }
jessekaiser 25:f45d61cc6dc6 131
jessekaiser 25:f45d61cc6dc6 132 void arm_mid ()
jessekaiser 25:f45d61cc6dc6 133 {
jessekaiser 25:f45d61cc6dc6 134 speed1_rad = -3.0; //positief is CCW, negatief CW (boven aanzicht)
jessekaiser 26:5981d4b1144e 135 setpoint1 = prev_setpoint1 + (TSAMP * speed1_rad);
jessekaiser 25:f45d61cc6dc6 136 if(setpoint1 > (155*2.0*PI/360)) { //setpoint in graden
jessekaiser 25:f45d61cc6dc6 137 setpoint1 = (155*2.0*PI/360);
jessekaiser 25:f45d61cc6dc6 138 }
jessekaiser 25:f45d61cc6dc6 139 if(setpoint1 < -(155*2.0*PI/360)) {
jessekaiser 25:f45d61cc6dc6 140 setpoint1 = -(155*2.0*PI/360);
jessekaiser 25:f45d61cc6dc6 141 }
jessekaiser 25:f45d61cc6dc6 142 prev_setpoint1 = setpoint1;
jessekaiser 25:f45d61cc6dc6 143 }
jessekaiser 25:f45d61cc6dc6 144
jessekaiser 25:f45d61cc6dc6 145 void arm_laag ()
jessekaiser 25:f45d61cc6dc6 146 {
jessekaiser 27:77bb255753be 147
jessekaiser 27:77bb255753be 148 speed1_rad = 6.0; //positief is CCW, negatief CW (boven aanzicht)
jessekaiser 27:77bb255753be 149 setpoint1 = prev_setpoint1 + TSAMP * speed1_rad;
jessekaiser 27:77bb255753be 150 if(setpoint1 > (155.0*2.0*PI/360)) { //setpoint in graden
jessekaiser 27:77bb255753be 151 setpoint1 = (155.0*2.0*PI/360);
jessekaiser 25:f45d61cc6dc6 152 }
jessekaiser 27:77bb255753be 153 if(setpoint1 < -(155.0*2.0*PI/360)) {
jessekaiser 27:77bb255753be 154 setpoint1 = -(155.0*2.0*PI/360);
jessekaiser 25:f45d61cc6dc6 155 }
jessekaiser 27:77bb255753be 156 prev_setpoint1 = setpoint1;
jessekaiser 25:f45d61cc6dc6 157 }
jessekaiser 25:f45d61cc6dc6 158
jessekaiser 27:77bb255753be 159 void arm_begin ()
jessekaiser 27:77bb255753be 160 {
jessekaiser 27:77bb255753be 161
jessekaiser 27:77bb255753be 162 speed1_rad = 1.0; //positief is CCW, negatief CW (boven aanzicht)
jessekaiser 27:77bb255753be 163 setpoint1 = prev_setpoint1 + TSAMP * speed1_rad;
jessekaiser 27:77bb255753be 164 if(setpoint1 > (0.0*2.0*PI/360)) { //setpoint in graden
jessekaiser 27:77bb255753be 165 setpoint1 = (0.0*2.0*PI/360);
jessekaiser 27:77bb255753be 166 }
jessekaiser 27:77bb255753be 167 if(setpoint1 < -(0.0*2.0*PI/360)) {
jessekaiser 27:77bb255753be 168 setpoint1 = -(0.0*2.0*PI/360);
jessekaiser 27:77bb255753be 169 }
jessekaiser 27:77bb255753be 170 prev_setpoint1 = setpoint1;
jessekaiser 27:77bb255753be 171 }
jessekaiser 0:d5d3b731340c 172 int main()
jessekaiser 3:dcc4cebba0d7 173 {
jessekaiser 26:5981d4b1144e 174 int wait_iterator = 0;
jessekaiser 25:f45d61cc6dc6 175 pwm_motor1.period_us(100);
jessekaiser 25:f45d61cc6dc6 176 motor1.setPosition(0);
jessekaiser 25:f45d61cc6dc6 177 pwm_motor2.period_us(100);
jessekaiser 25:f45d61cc6dc6 178 motor2.setPosition(0);
jessekaiser 25:f45d61cc6dc6 179 pc.baud(115200);
jessekaiser 25:f45d61cc6dc6 180
jessekaiser 0:d5d3b731340c 181 while(1) {
jessekaiser 27:77bb255753be 182
jessekaiser 27:77bb255753be 183 pc.printf("%d, %f \r\n", motor1.getPosition(), motor2.getPosition());
jessekaiser 26:5981d4b1144e 184 //MOTOR1
jessekaiser 26:5981d4b1144e 185 cur_pos_motor1 = motor1.getPosition();
jessekaiser 27:77bb255753be 186 pos_motor1_rad = (float)cur_pos_motor1/(4128.0/(2.0*PI)); //moet 4128
jessekaiser 25:f45d61cc6dc6 187 pwm1_percentage = pid1(setpoint1, pos_motor1_rad);
jessekaiser 27:77bb255753be 188
jessekaiser 27:77bb255753be 189
jessekaiser 22:22cb158bcea4 190 if (pwm1_percentage < -1.0) {
jessekaiser 22:22cb158bcea4 191 pwm1_percentage = -1.0;
jessekaiser 24:7fbd904d191c 192 }
jessekaiser 24:7fbd904d191c 193 if (pwm1_percentage > 1.0) {
jessekaiser 24:7fbd904d191c 194 pwm1_percentage = 1.0;
jessekaiser 22:22cb158bcea4 195 }
jessekaiser 27:77bb255753be 196
jessekaiser 26:5981d4b1144e 197 pwm_motor1.write(abs(pwm1_percentage));
jessekaiser 24:7fbd904d191c 198 if(pwm1_percentage > 0) {
jessekaiser 24:7fbd904d191c 199 motordir1 = 0;
jessekaiser 24:7fbd904d191c 200 } else {
jessekaiser 22:22cb158bcea4 201 motordir1 = 1;
jessekaiser 24:7fbd904d191c 202 }
jessekaiser 27:77bb255753be 203
jessekaiser 27:77bb255753be 204 //MOTOR 2
jessekaiser 27:77bb255753be 205 cur_pos_motor2 = motor2.getPosition();
jessekaiser 27:77bb255753be 206 pos_motor2_rad = (float)cur_pos_motor2/(3200.0/(2.0*PI)); //moet 4128
jessekaiser 27:77bb255753be 207 pwm2_percentage = pid2(setpoint2, pos_motor2_rad);
jessekaiser 27:77bb255753be 208
jessekaiser 27:77bb255753be 209 if (pwm2_percentage < -1.0) {
jessekaiser 27:77bb255753be 210 pwm2_percentage = -1.0;
jessekaiser 27:77bb255753be 211 }
jessekaiser 27:77bb255753be 212 if (pwm2_percentage > 1.0) {
jessekaiser 27:77bb255753be 213 pwm2_percentage = 1.0;
jessekaiser 27:77bb255753be 214 }
jessekaiser 27:77bb255753be 215
jessekaiser 27:77bb255753be 216 pwm_motor2.write(abs(pwm2_percentage));
jessekaiser 27:77bb255753be 217 if(pwm2_percentage > 0) {
jessekaiser 27:77bb255753be 218 motordir2 = 0;
jessekaiser 27:77bb255753be 219 } else {
jessekaiser 27:77bb255753be 220 motordir2 = 1;
jessekaiser 27:77bb255753be 221 }
jessekaiser 27:77bb255753be 222
jessekaiser 26:5981d4b1144e 223 wait(TSAMP);
jessekaiser 27:77bb255753be 224
jessekaiser 27:77bb255753be 225
jessekaiser 26:5981d4b1144e 226 //STATES
jessekaiser 27:77bb255753be 227 if(staat1 == 0) {
jessekaiser 25:f45d61cc6dc6 228 arm_hoog();
jessekaiser 26:5981d4b1144e 229 wait_iterator = 0;
jessekaiser 25:f45d61cc6dc6 230 }
jessekaiser 27:77bb255753be 231 else if(staat1 == 1)
jessekaiser 26:5981d4b1144e 232 {
jessekaiser 26:5981d4b1144e 233 wait_iterator++;
jessekaiser 26:5981d4b1144e 234 if(wait_iterator > 200)
jessekaiser 27:77bb255753be 235 staat1 = 2;
jessekaiser 26:5981d4b1144e 236 }
jessekaiser 26:5981d4b1144e 237 else
jessekaiser 25:f45d61cc6dc6 238 {
jessekaiser 27:77bb255753be 239 arm_begin();
jessekaiser 24:7fbd904d191c 240 }
jessekaiser 25:f45d61cc6dc6 241 }
jessekaiser 0:d5d3b731340c 242 }
jessekaiser 18:fb85c58a4106 243
jessekaiser 18:fb85c58a4106 244