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@10:48d309d9bb1c, 2014-10-30 (annotated)

Committer:
Hooglugt
Date:
Thu Oct 30 12:21:21 2014 +0000
Revision:
10:48d309d9bb1c
Parent:
9:58f9e4f8229c
Child:
11:51e29f3d4545
na slag gaat arm terug naar beginpositie - error: motor1 op zn weg terug stottert

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 10:48d309d9bb1c 7 HIDScope scope(4);
Hooglugt 8:5dab7ea40bc1 8 volatile bool looptimerflag;
Hooglugt 10:48d309d9bb1c 9 Ticker looptimer;
Hooglugt 8:5dab7ea40bc1 10
Hooglugt 8:5dab7ea40bc1 11 void setlooptimerflag(void)
Hooglugt 8:5dab7ea40bc1 12 {
Hooglugt 8:5dab7ea40bc1 13 looptimerflag = true;
Hooglugt 10:48d309d9bb1c 14
Hooglugt 8:5dab7ea40bc1 15 }
Hooglugt 8:5dab7ea40bc1 16
Hooglugt 8:5dab7ea40bc1 17 void keep_in_range(float * in, float min, float max)
Hooglugt 8:5dab7ea40bc1 18 {
Hooglugt 8:5dab7ea40bc1 19 *in > min ? *in < max? : *in = max: *in = max;
Hooglugt 8:5dab7ea40bc1 20 }
wbuysman 0:c205dc094877 21
Hooglugt 10:48d309d9bb1c 22 //motor 1, voltage pins op M2
Hooglugt 10:48d309d9bb1c 23 Encoder motor1(PTD3, PTD5);
Hooglugt 10:48d309d9bb1c 24 DigitalOut motor1dir(PTC9);
Hooglugt 10:48d309d9bb1c 25 PwmOut pwm_motor1(PTC8);
wbuysman 5:7fb05dfead4d 26
Hooglugt 10:48d309d9bb1c 27 //motor 2,
Hooglugt 10:48d309d9bb1c 28 Encoder motor2(PTD2,PTD0);
Hooglugt 10:48d309d9bb1c 29 DigitalOut motor2dir(PTA4);
Hooglugt 10:48d309d9bb1c 30 PwmOut pwm_motor2(PTA5);
Hooglugt 10:48d309d9bb1c 31
Hooglugt 10:48d309d9bb1c 32 float integral = 0;
Hooglugt 10:48d309d9bb1c 33 float batjeset = 0; //een if statement wordt "true" wanneer batje voor een bepaalde tijd de juiste hoek heeft behouden
Hooglugt 10:48d309d9bb1c 34 float balhit = 0; //balhit wordt 1 wanneer arm een bepaalde hoek heeft afgelegd
Hooglugt 10:48d309d9bb1c 35
Hooglugt 8:5dab7ea40bc1 36 float setpoint1 = 8; // te behalen speed van motor1 (37D)
Hooglugt 8:5dab7ea40bc1 37 float dt1 = 0.01;
Hooglugt 8:5dab7ea40bc1 38 float Kp1 = 1.10; //DEZE KP1 EN KP1 ZIJN NOG NIET DEFINITIEF
Hooglugt 8:5dab7ea40bc1 39 float Ki1 = 0.20;
Hooglugt 8:5dab7ea40bc1 40 float error1 = 0;
Hooglugt 8:5dab7ea40bc1 41 float pwm1 = 0;
Hooglugt 8:5dab7ea40bc1 42 float omrekenfactor1 = 0.0028035714; // 6.28/(32*70)
Hooglugt 8:5dab7ea40bc1 43
Hooglugt 8:5dab7ea40bc1 44 float setpoint2 = 3.14; // te behalen hoek van motor2 (25D)
Hooglugt 8:5dab7ea40bc1 45 float dt2 = 0.01;
Hooglugt 8:5dab7ea40bc1 46 float Kp2 = 0.30;
Hooglugt 8:5dab7ea40bc1 47 float Ki2 = 0.20;
Hooglugt 8:5dab7ea40bc1 48 float error2 = 0;
Hooglugt 8:5dab7ea40bc1 49 float pwm2 = 0;
Hooglugt 8:5dab7ea40bc1 50 float omrekenfactor2 = 0.0015213178; // 6.28/(24*172);
Hooglugt 8:5dab7ea40bc1 51
wbuysman 0:c205dc094877 52 int main()
wbuysman 0:c205dc094877 53 {
Hooglugt 10:48d309d9bb1c 54 looptimer.attach(setlooptimerflag,TSAMP);
wbuysman 0:c205dc094877 55 pwm_motor1.period_us(100); //10kHz PWM frequency
wbuysman 0:c205dc094877 56 pwm_motor2.period_us(100); //10kHz PWM frequency
wbuysman 0:c205dc094877 57
Hooglugt 10:48d309d9bb1c 58 // nog een goto motor2control;
Hooglugt 10:48d309d9bb1c 59 // motor2control:
Hooglugt 8:5dab7ea40bc1 60 while(1) { // loop voor het goed plaatsen van motor2 (batje hoek)
Hooglugt 8:5dab7ea40bc1 61 while(!looptimerflag);
Hooglugt 8:5dab7ea40bc1 62 looptimerflag = false; //clear flag
wbuysman 6:2887ad4c5d97 63
Hooglugt 8:5dab7ea40bc1 64 //regelaar motor2, bepaalt positie
Hooglugt 8:5dab7ea40bc1 65 error2 = setpoint2 - motor2.getPosition()*omrekenfactor2;
Hooglugt 10:48d309d9bb1c 66 integral = integral + error2*TSAMP;
Hooglugt 10:48d309d9bb1c 67 pwm2 = Kp2*error2 + Ki2*integral;
Hooglugt 10:48d309d9bb1c 68
Hooglugt 8:5dab7ea40bc1 69 keep_in_range(&pwm2, -1,1);
Hooglugt 8:5dab7ea40bc1 70 pwm_motor2.write(abs(pwm2));
Hooglugt 10:48d309d9bb1c 71 if(pwm2 > 0) {
Hooglugt 8:5dab7ea40bc1 72 motor2dir = 1;
Hooglugt 10:48d309d9bb1c 73 } else {
Hooglugt 8:5dab7ea40bc1 74 motor2dir = 0;
Hooglugt 10:48d309d9bb1c 75 }
wbuysman 7:59116528d895 76
Hooglugt 8:5dab7ea40bc1 77 //controleert of batje positie heeft bepaald
Hooglugt 10:48d309d9bb1c 78 if(batjeset < 200) { // dit is nog te bepalen, op dit moment als binnen marge van 1% voor 2 seconde, dan naar volgende motorcontrol
Hooglugt 8:5dab7ea40bc1 79 if (motor2.getPosition()*omrekenfactor2 > setpoint2*1.01 || motor2.getPosition()*omrekenfactor2 < setpoint2*0.99) {
Hooglugt 8:5dab7ea40bc1 80 batjeset = 0;
Hooglugt 8:5dab7ea40bc1 81 } else {
Hooglugt 8:5dab7ea40bc1 82 batjeset++;
Hooglugt 8:5dab7ea40bc1 83 }
wbuysman 7:59116528d895 84 } else {
Hooglugt 8:5dab7ea40bc1 85 pwm_motor2.write(0);
Hooglugt 10:48d309d9bb1c 86 batjeset = integral = 0;
Hooglugt 10:48d309d9bb1c 87 wait(2);
Hooglugt 8:5dab7ea40bc1 88 goto motor1control;
wbuysman 6:2887ad4c5d97 89 }
Hooglugt 8:5dab7ea40bc1 90 }
wbuysman 3:e40763e01a80 91
Hooglugt 8:5dab7ea40bc1 92 motor1control:
Hooglugt 8:5dab7ea40bc1 93 while(1) { // loop voor het goed plaatsen van motor1 (batje snelheid)
Hooglugt 8:5dab7ea40bc1 94 while(!looptimerflag);
Hooglugt 8:5dab7ea40bc1 95 looptimerflag = false; //clear flag
Hooglugt 8:5dab7ea40bc1 96
Hooglugt 9:58f9e4f8229c 97 if (balhit == 0) { //regelaar motor1, bepaalt snelheid
Hooglugt 8:5dab7ea40bc1 98 error1 = setpoint1 - motor1.getSpeed()*omrekenfactor1;
Hooglugt 10:48d309d9bb1c 99 integral = integral + error1*TSAMP;
Hooglugt 10:48d309d9bb1c 100 pwm1 = Kp1*error1 + Ki1*integral;
Hooglugt 9:58f9e4f8229c 101 } else { //regelaar motor1, bepaalt positie
Hooglugt 10:48d309d9bb1c 102 pwm_motor1.write(0);
Hooglugt 10:48d309d9bb1c 103 balhit = integral = 0;
Hooglugt 10:48d309d9bb1c 104 wait(2); // wait voordat arm weer naar beginpositie terugkeert
Hooglugt 10:48d309d9bb1c 105 goto resetpositionmotor1;
wbuysman 7:59116528d895 106 }
wbuysman 7:59116528d895 107
Hooglugt 9:58f9e4f8229c 108 keep_in_range(&pwm1, -1,1);
Hooglugt 10:48d309d9bb1c 109
Hooglugt 10:48d309d9bb1c 110 if(pwm1 > 0) {
Hooglugt 8:5dab7ea40bc1 111 motor1dir = 1;
Hooglugt 10:48d309d9bb1c 112 } else {
Hooglugt 8:5dab7ea40bc1 113 motor1dir = 0;
Hooglugt 10:48d309d9bb1c 114 }
Hooglugt 10:48d309d9bb1c 115
Hooglugt 10:48d309d9bb1c 116 pwm_motor1.write(abs(pwm1));
Hooglugt 10:48d309d9bb1c 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 10:48d309d9bb1c 121 balhit = 1;
Hooglugt 8:5dab7ea40bc1 122 }
wbuysman 0:c205dc094877 123 }
Hooglugt 10:48d309d9bb1c 124
Hooglugt 10:48d309d9bb1c 125 resetpositionmotor1:
Hooglugt 10:48d309d9bb1c 126 while(1) { // slagarm wordt weer in oorspronkelijke positie geplaatst
Hooglugt 10:48d309d9bb1c 127 while(!looptimerflag);
Hooglugt 10:48d309d9bb1c 128 looptimerflag = false; //clear flag
Hooglugt 9:58f9e4f8229c 129
Hooglugt 10:48d309d9bb1c 130 //regelaar motor1, bepaalt positie
Hooglugt 10:48d309d9bb1c 131 error1 = -1*motor1.getPosition()*omrekenfactor1;
Hooglugt 10:48d309d9bb1c 132 integral = integral + error1*TSAMP;
Hooglugt 10:48d309d9bb1c 133 pwm1 = Kp2*error1 + Ki2*integral;
Hooglugt 10:48d309d9bb1c 134
Hooglugt 10:48d309d9bb1c 135 keep_in_range(&pwm1, -1,1);
Hooglugt 10:48d309d9bb1c 136
Hooglugt 10:48d309d9bb1c 137 if(pwm1 > 0) { //HIER MOET NAAR GEKEKEN WORDEN!! de if-loop moet motor1dir op 0 zetten, maar doet dit niet, waarom?
Hooglugt 10:48d309d9bb1c 138 motor1dir = 0;
Hooglugt 10:48d309d9bb1c 139 } else {
Hooglugt 10:48d309d9bb1c 140 motor1dir = 1;
Hooglugt 10:48d309d9bb1c 141 }
Hooglugt 10:48d309d9bb1c 142
Hooglugt 10:48d309d9bb1c 143 pwm_motor1.write(abs(pwm1));
Hooglugt 10:48d309d9bb1c 144
Hooglugt 10:48d309d9bb1c 145 //controleert of arm terug in positie is
Hooglugt 10:48d309d9bb1c 146 if(batjeset < 200) {
Hooglugt 10:48d309d9bb1c 147 if (motor1.getPosition()*omrekenfactor1 > 0.1 || motor1.getPosition()*omrekenfactor1 < -0.1) {
Hooglugt 10:48d309d9bb1c 148 batjeset = 0;
Hooglugt 10:48d309d9bb1c 149 } else {
Hooglugt 10:48d309d9bb1c 150 batjeset++;
Hooglugt 10:48d309d9bb1c 151 }
Hooglugt 10:48d309d9bb1c 152 } else {
Hooglugt 10:48d309d9bb1c 153 pwm_motor1.write(0);
Hooglugt 10:48d309d9bb1c 154 batjeset = integral = 0;
Hooglugt 10:48d309d9bb1c 155 wait(5);
Hooglugt 10:48d309d9bb1c 156 goto resetpositionmotor2;
Hooglugt 10:48d309d9bb1c 157 }
Hooglugt 10:48d309d9bb1c 158 }
Hooglugt 10:48d309d9bb1c 159
Hooglugt 10:48d309d9bb1c 160 resetpositionmotor2:
Hooglugt 10:48d309d9bb1c 161 while(1) { // loop voor het goed plaatsen van motor2 (batje hoek)
Hooglugt 9:58f9e4f8229c 162 while(!looptimerflag);
Hooglugt 9:58f9e4f8229c 163 looptimerflag = false; //clear flag
Hooglugt 9:58f9e4f8229c 164
Hooglugt 9:58f9e4f8229c 165 //regelaar motor2, bepaalt positie
Hooglugt 10:48d309d9bb1c 166 error2 = -1*motor2.getPosition()*omrekenfactor2;
Hooglugt 10:48d309d9bb1c 167 integral = integral + error2*TSAMP;
Hooglugt 10:48d309d9bb1c 168 pwm2 = Kp2*error2 + Ki2*integral;
Hooglugt 9:58f9e4f8229c 169
Hooglugt 9:58f9e4f8229c 170 keep_in_range(&pwm2, -1,1);
Hooglugt 9:58f9e4f8229c 171
Hooglugt 10:48d309d9bb1c 172 if(pwm2 > 0) {
Hooglugt 10:48d309d9bb1c 173 motor2dir = 1;
Hooglugt 10:48d309d9bb1c 174 } else {
Hooglugt 10:48d309d9bb1c 175 motor2dir = 0;
Hooglugt 10:48d309d9bb1c 176 }
Hooglugt 10:48d309d9bb1c 177
Hooglugt 10:48d309d9bb1c 178 pwm_motor2.write(abs(pwm2));
Hooglugt 9:58f9e4f8229c 179
Hooglugt 10:48d309d9bb1c 180 //controleert of batje positie heeft bepaald
Hooglugt 10:48d309d9bb1c 181 if(batjeset < 200) { // dit is nog te bepalen, op dit moment als binnen marge van 1% voor 2 seconde, dan naar volgende motorcontrol
Hooglugt 10:48d309d9bb1c 182 if (motor2.getPosition()*omrekenfactor2 > 0.03 || motor2.getPosition()*omrekenfactor2 < -0.03) {
Hooglugt 9:58f9e4f8229c 183 batjeset = 0;
Hooglugt 9:58f9e4f8229c 184 } else {
Hooglugt 9:58f9e4f8229c 185 batjeset++;
Hooglugt 9:58f9e4f8229c 186 }
Hooglugt 9:58f9e4f8229c 187 } else {
Hooglugt 9:58f9e4f8229c 188 pwm_motor2.write(0);
Hooglugt 10:48d309d9bb1c 189 batjeset = integral = 0;
Hooglugt 10:48d309d9bb1c 190 wait(2);
Hooglugt 10:48d309d9bb1c 191 goto test;
Hooglugt 9:58f9e4f8229c 192 //direction = force = 0;
Hooglugt 9:58f9e4f8229c 193 //goto directionchoice;
Hooglugt 10:48d309d9bb1c 194 }
Hooglugt 9:58f9e4f8229c 195 }
Hooglugt 9:58f9e4f8229c 196 test:
Hooglugt 9:58f9e4f8229c 197 wait(2);
wbuysman 0:c205dc094877 198 }