Aukie Hooglugt / Mbed 2 deprecated Regelaar_motor

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 HIDScope mbed

main.cpp@9:58f9e4f8229c, 2014-10-30 (annotated)

Committer:
Hooglugt
Date:
Thu Oct 30 09:07:45 2014 +0000
Revision:
9:58f9e4f8229c
Parent:
8:5dab7ea40bc1
Child:
10:48d309d9bb1c
terug naar reset positie toegevoegd

Who changed what in which revision?

UserRevisionLine numberNew contents of line
wbuysman 0:c205dc094877 1 #include "mbed.h"
wbuysman 0:c205dc094877 2 #include "encoder.h"
wbuysman 0:c205dc094877 3 #include "HIDScope.h"
wbuysman 0:c205dc094877 4
Hooglugt 8:5dab7ea40bc1 5 #define TSAMP 0.001
wbuysman 0:c205dc094877 6
Hooglugt 8:5dab7ea40bc1 7 volatile bool looptimerflag;
Hooglugt 8:5dab7ea40bc1 8 HIDScope scope(4);
Hooglugt 8:5dab7ea40bc1 9
Hooglugt 8:5dab7ea40bc1 10 void setlooptimerflag(void)
Hooglugt 8:5dab7ea40bc1 11 {
Hooglugt 8:5dab7ea40bc1 12 looptimerflag = true;
Hooglugt 8:5dab7ea40bc1 13 }
Hooglugt 8:5dab7ea40bc1 14
Hooglugt 8:5dab7ea40bc1 15 void keep_in_range(float * in, float min, float max)
Hooglugt 8:5dab7ea40bc1 16 {
Hooglugt 8:5dab7ea40bc1 17 *in > min ? *in < max? : *in = max: *in = max;
Hooglugt 8:5dab7ea40bc1 18 }
wbuysman 0:c205dc094877 19
wbuysman 5:7fb05dfead4d 20 void keep_in_range(float * in, float min, float max);
wbuysman 5:7fb05dfead4d 21
Hooglugt 8:5dab7ea40bc1 22 float integral1 = 0;
Hooglugt 8:5dab7ea40bc1 23 float setpoint1 = 8; // te behalen speed van motor1 (37D)
Hooglugt 8:5dab7ea40bc1 24 float dt1 = 0.01;
Hooglugt 8:5dab7ea40bc1 25 float Kp1 = 1.10; //DEZE KP1 EN KP1 ZIJN NOG NIET DEFINITIEF
Hooglugt 8:5dab7ea40bc1 26 float Ki1 = 0.20;
Hooglugt 8:5dab7ea40bc1 27 float error1 = 0;
Hooglugt 8:5dab7ea40bc1 28 float pwm1 = 0;
Hooglugt 8:5dab7ea40bc1 29 float omrekenfactor1 = 0.0028035714; // 6.28/(32*70)
Hooglugt 8:5dab7ea40bc1 30
Hooglugt 8:5dab7ea40bc1 31 float integral2 = 0;
Hooglugt 8:5dab7ea40bc1 32 float setpoint2 = 3.14; // te behalen hoek van motor2 (25D)
Hooglugt 8:5dab7ea40bc1 33 float dt2 = 0.01;
Hooglugt 8:5dab7ea40bc1 34 float Kp2 = 0.30;
Hooglugt 8:5dab7ea40bc1 35 float Ki2 = 0.20;
Hooglugt 8:5dab7ea40bc1 36 float error2 = 0;
Hooglugt 8:5dab7ea40bc1 37 float pwm2 = 0;
Hooglugt 8:5dab7ea40bc1 38 float omrekenfactor2 = 0.0015213178; // 6.28/(24*172);
Hooglugt 8:5dab7ea40bc1 39
wbuysman 0:c205dc094877 40 int main()
wbuysman 0:c205dc094877 41 {
Hooglugt 8:5dab7ea40bc1 42 wait(5);
Hooglugt 8:5dab7ea40bc1 43 //motor 1,
wbuysman 0:c205dc094877 44 Encoder motor1(PTD3, PTD5);
wbuysman 0:c205dc094877 45 DigitalOut motor1dir(PTC9);
wbuysman 0:c205dc094877 46 PwmOut pwm_motor1(PTC8);
wbuysman 0:c205dc094877 47 pwm_motor1.period_us(100); //10kHz PWM frequency
wbuysman 0:c205dc094877 48
Hooglugt 8:5dab7ea40bc1 49 //motor 2,
wbuysman 0:c205dc094877 50 Encoder motor2(PTD2,PTD0);
wbuysman 0:c205dc094877 51 DigitalOut motor2dir(PTA4);
wbuysman 0:c205dc094877 52 PwmOut pwm_motor2(PTA5);
wbuysman 0:c205dc094877 53 pwm_motor2.period_us(100); //10kHz PWM frequency
wbuysman 0:c205dc094877 54
Hooglugt 8:5dab7ea40bc1 55 Ticker looptimer;
Hooglugt 8:5dab7ea40bc1 56 looptimer.attach(setlooptimerflag,TSAMP);
wbuysman 0:c205dc094877 57
Hooglugt 8:5dab7ea40bc1 58 float batjeset = 0; //een if statement wordt "true" wanneer batje voor een bepaalde tijd de juiste hoek heeft behouden
Hooglugt 8:5dab7ea40bc1 59 float balhit = 0; //balhit wordt 1 wanneer arm een bepaalde hoek heeft afgelegd
Hooglugt 8:5dab7ea40bc1 60
Hooglugt 8:5dab7ea40bc1 61 while(1) { // loop voor het goed plaatsen van motor2 (batje hoek)
Hooglugt 8:5dab7ea40bc1 62 while(!looptimerflag);
Hooglugt 8:5dab7ea40bc1 63 looptimerflag = false; //clear flag
wbuysman 6:2887ad4c5d97 64
Hooglugt 8:5dab7ea40bc1 65 //regelaar motor2, bepaalt positie
Hooglugt 8:5dab7ea40bc1 66 error2 = setpoint2 - motor2.getPosition()*omrekenfactor2;
Hooglugt 8:5dab7ea40bc1 67 integral2 = integral2 + error2*TSAMP;
Hooglugt 8:5dab7ea40bc1 68 pwm2 = Kp2*error2 + Ki2*integral2;
Hooglugt 8:5dab7ea40bc1 69 keep_in_range(&pwm2, -1,1);
Hooglugt 8:5dab7ea40bc1 70 pwm_motor2.write(abs(pwm2));
Hooglugt 8:5dab7ea40bc1 71 if(pwm2 > 0)
Hooglugt 8:5dab7ea40bc1 72 motor2dir = 1;
Hooglugt 8:5dab7ea40bc1 73 else
Hooglugt 8:5dab7ea40bc1 74 motor2dir = 0;
wbuysman 7:59116528d895 75
Hooglugt 8:5dab7ea40bc1 76 //controleert of batje positie heeft bepaald
Hooglugt 8:5dab7ea40bc1 77 if(batjeset < 200) { // dit is nog te bepalen, op dit moment als binnen marge van 5% voor 2 seconde, dan naar volgende motorcontrol
Hooglugt 8:5dab7ea40bc1 78 if (motor2.getPosition()*omrekenfactor2 > setpoint2*1.01 || motor2.getPosition()*omrekenfactor2 < setpoint2*0.99) {
Hooglugt 8:5dab7ea40bc1 79 batjeset = 0;
Hooglugt 8:5dab7ea40bc1 80 } else {
Hooglugt 8:5dab7ea40bc1 81 batjeset++;
Hooglugt 8:5dab7ea40bc1 82 }
wbuysman 7:59116528d895 83 } else {
Hooglugt 8:5dab7ea40bc1 84 pwm_motor2.write(0);
Hooglugt 8:5dab7ea40bc1 85 goto motor1control;
wbuysman 6:2887ad4c5d97 86 }
Hooglugt 8:5dab7ea40bc1 87 }
wbuysman 3:e40763e01a80 88
Hooglugt 8:5dab7ea40bc1 89 motor1control:
Hooglugt 8:5dab7ea40bc1 90 while(1) { // loop voor het goed plaatsen van motor1 (batje snelheid)
Hooglugt 8:5dab7ea40bc1 91 while(!looptimerflag);
Hooglugt 8:5dab7ea40bc1 92 looptimerflag = false; //clear flag
Hooglugt 8:5dab7ea40bc1 93
Hooglugt 9:58f9e4f8229c 94 if (balhit == 0) { //regelaar motor1, bepaalt snelheid
Hooglugt 8:5dab7ea40bc1 95 error1 = setpoint1 - motor1.getSpeed()*omrekenfactor1;
Hooglugt 8:5dab7ea40bc1 96 integral1 = integral1 + error1*TSAMP;
Hooglugt 8:5dab7ea40bc1 97 pwm1 = Kp1*error1 + Ki1*integral1;
Hooglugt 9:58f9e4f8229c 98 } else { //regelaar motor1, bepaalt positie
Hooglugt 9:58f9e4f8229c 99 batjeset = integral1 = integral2 = 0;
Hooglugt 9:58f9e4f8229c 100 goto resetposition;
wbuysman 7:59116528d895 101 }
wbuysman 7:59116528d895 102
Hooglugt 9:58f9e4f8229c 103 keep_in_range(&pwm1, -1,1);
Hooglugt 9:58f9e4f8229c 104 pwm_motor1.write(abs(pwm1));
Hooglugt 9:58f9e4f8229c 105
Hooglugt 8:5dab7ea40bc1 106 if(pwm1 > 0)
Hooglugt 8:5dab7ea40bc1 107 motor1dir = 1;
Hooglugt 8:5dab7ea40bc1 108 else
Hooglugt 8:5dab7ea40bc1 109 motor1dir = 0;
wbuysman 6:2887ad4c5d97 110
Hooglugt 8:5dab7ea40bc1 111 //scope set
Hooglugt 8:5dab7ea40bc1 112 scope.set(0, pwm1);
Hooglugt 8:5dab7ea40bc1 113 scope.set(1, motor1.getSpeed()*omrekenfactor1);
Hooglugt 8:5dab7ea40bc1 114 scope.set(2, motor1.getPosition()*omrekenfactor1);
Hooglugt 8:5dab7ea40bc1 115 scope.set(3, balhit);
Hooglugt 8:5dab7ea40bc1 116 scope.send();
wbuysman 6:2887ad4c5d97 117
Hooglugt 8:5dab7ea40bc1 118 //controleert of batje balletje heeft bereikt
Hooglugt 9:58f9e4f8229c 119 //if (motor1.getSpeed()*omrekenfactor1 >= 7.5 && motor1.getPosition()*omrekenfactor1 > 1.03 && motor1.getPosition()*omrekenfactor1 < 1.07) { vrij specifieke if-statement ter controle
Hooglugt 9:58f9e4f8229c 120 if (motor1.getPosition()*omrekenfactor1 > 1.10) {
Hooglugt 8:5dab7ea40bc1 121 balhit = 1;
Hooglugt 8:5dab7ea40bc1 122 }
Hooglugt 9:58f9e4f8229c 123
wbuysman 0:c205dc094877 124 }
Hooglugt 9:58f9e4f8229c 125 resetposition:
Hooglugt 9:58f9e4f8229c 126
Hooglugt 9:58f9e4f8229c 127 while(1) { // batje en slagarm worden weer in oorspronkelijke positie geplaatst
Hooglugt 9:58f9e4f8229c 128 while(!looptimerflag);
Hooglugt 9:58f9e4f8229c 129 looptimerflag = false; //clear flag
Hooglugt 9:58f9e4f8229c 130
Hooglugt 9:58f9e4f8229c 131 //regelaar motor2, bepaalt positie
Hooglugt 9:58f9e4f8229c 132 error2 = motor2.getPosition()*omrekenfactor2;
Hooglugt 9:58f9e4f8229c 133 integral2 = integral2 + error2*TSAMP;
Hooglugt 9:58f9e4f8229c 134 pwm2 = Kp2*error2 + Ki2*integral2;
Hooglugt 9:58f9e4f8229c 135
Hooglugt 9:58f9e4f8229c 136 keep_in_range(&pwm2, -1,1);
Hooglugt 9:58f9e4f8229c 137 pwm_motor2.write(abs(pwm2));
Hooglugt 9:58f9e4f8229c 138 if(pwm2 > 0)
Hooglugt 9:58f9e4f8229c 139 motor2dir = 1;
Hooglugt 9:58f9e4f8229c 140 else
Hooglugt 9:58f9e4f8229c 141 motor2dir = 0;
Hooglugt 9:58f9e4f8229c 142
Hooglugt 9:58f9e4f8229c 143 //regelaar motor1, bepaalt positie
Hooglugt 9:58f9e4f8229c 144 error1 = motor1.getPosition()*omrekenfactor1;
Hooglugt 9:58f9e4f8229c 145 integral1 = integral1 + error1*TSAMP;
Hooglugt 9:58f9e4f8229c 146 pwm1 = Kp2*error1 + Ki2*integral1; //kp2 en ki2 worden hier gekozen (voorlopig), goede insteling voor motor2 om goed positie te bepalen, miss ook voor motor1
Hooglugt 9:58f9e4f8229c 147
Hooglugt 9:58f9e4f8229c 148 keep_in_range(&pwm1, -1,1);
Hooglugt 9:58f9e4f8229c 149 pwm_motor1.write(abs(pwm1));
Hooglugt 9:58f9e4f8229c 150 if(pwm1 > 0)
Hooglugt 9:58f9e4f8229c 151 motor1dir = 1;
Hooglugt 9:58f9e4f8229c 152 else
Hooglugt 9:58f9e4f8229c 153 motor1dir = 0;
Hooglugt 9:58f9e4f8229c 154
Hooglugt 9:58f9e4f8229c 155 //controleert of robot terug in oorspronkelijke positie is
Hooglugt 9:58f9e4f8229c 156 if(batjeset < 200) {
Hooglugt 9:58f9e4f8229c 157 if (motor2.getPosition()*omrekenfactor2 > 0.03 || motor2.getPosition()*omrekenfactor2 < -0.03 || motor1.getPosition()*omrekenfactor1 < -0.03 || motor1.getPosition()*omrekenfactor1 >0.03) {
Hooglugt 9:58f9e4f8229c 158 batjeset = 0;
Hooglugt 9:58f9e4f8229c 159 } else {
Hooglugt 9:58f9e4f8229c 160 batjeset++;
Hooglugt 9:58f9e4f8229c 161 }
Hooglugt 9:58f9e4f8229c 162 } else {
Hooglugt 9:58f9e4f8229c 163 pwm_motor2.write(0);
Hooglugt 9:58f9e4f8229c 164 pwm_motor2.write(0);
Hooglugt 9:58f9e4f8229c 165 //direction = force = 0;
Hooglugt 9:58f9e4f8229c 166 //goto directionchoice;
Hooglugt 9:58f9e4f8229c 167 goto test;
Hooglugt 9:58f9e4f8229c 168 }
Hooglugt 9:58f9e4f8229c 169 }
Hooglugt 9:58f9e4f8229c 170 test:
Hooglugt 9:58f9e4f8229c 171 wait(2);
Hooglugt 9:58f9e4f8229c 172
wbuysman 0:c205dc094877 173 }