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@8:5dab7ea40bc1, 2014-10-29 (annotated)

Committer:
Hooglugt
Date:
Wed Oct 29 15:07:47 2014 +0000
Revision:
8:5dab7ea40bc1
Parent:
7:59116528d895
Child:
9:58f9e4f8229c
aansturing motor1 en motor2 gedaan, kp1 en ki1 zijn nog niet goed ingesteld --> hierdoor haalt motor1 niet op 60 graden de correcte snelheid - kalibratie ontbreekt nog

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 8:5dab7ea40bc1 94 //regelaar motor1, bepaalt snelheid
Hooglugt 8:5dab7ea40bc1 95 if (balhit == 0) {
Hooglugt 8:5dab7ea40bc1 96 error1 = setpoint1 - motor1.getSpeed()*omrekenfactor1;
Hooglugt 8:5dab7ea40bc1 97 integral1 = integral1 + error1*TSAMP;
Hooglugt 8:5dab7ea40bc1 98 pwm1 = Kp1*error1 + Ki1*integral1;
Hooglugt 8:5dab7ea40bc1 99 keep_in_range(&pwm1, -1,1);
Hooglugt 8:5dab7ea40bc1 100 pwm_motor1.write(abs(pwm1));
wbuysman 3:e40763e01a80 101 } else {
Hooglugt 8:5dab7ea40bc1 102 pwm_motor1.write(0);
Hooglugt 8:5dab7ea40bc1 103 goto motorcontroldone;
wbuysman 7:59116528d895 104 }
wbuysman 7:59116528d895 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 8:5dab7ea40bc1 119 if (motor1.getSpeed()*omrekenfactor1 >= 7.5 && motor1.getPosition()*omrekenfactor1 > 1.03 && motor1.getPosition()*omrekenfactor1 < 1.07) {
Hooglugt 8:5dab7ea40bc1 120 balhit = 1;
Hooglugt 8:5dab7ea40bc1 121 }
wbuysman 0:c205dc094877 122 }
Hooglugt 8:5dab7ea40bc1 123 motorcontroldone:
Hooglugt 8:5dab7ea40bc1 124 wait(1);
wbuysman 0:c205dc094877 125 }