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@7:59116528d895, 2014-10-28 (annotated)

Committer:
wbuysman
Date:
Tue Oct 28 13:49:24 2014 +0000
Revision:
7:59116528d895
Parent:
6:2887ad4c5d97
Child:
8:5dab7ea40bc1
werkt niet

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
wbuysman 0:c205dc094877 5
wbuysman 6:2887ad4c5d97 6 HIDScope scope(6);
wbuysman 0:c205dc094877 7
wbuysman 5:7fb05dfead4d 8 void keep_in_range(float * in, float min, float max);
wbuysman 5:7fb05dfead4d 9
wbuysman 0:c205dc094877 10 int main()
wbuysman 0:c205dc094877 11 {
wbuysman 0:c205dc094877 12 //motor 1, 25D
wbuysman 0:c205dc094877 13 Encoder motor1(PTD3, PTD5);
wbuysman 0:c205dc094877 14 DigitalOut motor1dir(PTC9);
wbuysman 0:c205dc094877 15 PwmOut pwm_motor1(PTC8);
wbuysman 0:c205dc094877 16 pwm_motor1.period_us(100); //10kHz PWM frequency
wbuysman 0:c205dc094877 17
wbuysman 0:c205dc094877 18 //motor 2, 25D
wbuysman 0:c205dc094877 19 Encoder motor2(PTD2,PTD0);
wbuysman 0:c205dc094877 20 DigitalOut motor2dir(PTA4);
wbuysman 0:c205dc094877 21 PwmOut pwm_motor2(PTA5);
wbuysman 0:c205dc094877 22 pwm_motor2.period_us(100); //10kHz PWM frequency
wbuysman 0:c205dc094877 23
wbuysman 0:c205dc094877 24
wbuysman 2:302174acdac7 25 float integral1 = 0;
wbuysman 7:59116528d895 26 float setpoint1 = 8;
wbuysman 7:59116528d895 27 float dt1 = 0.001;
wbuysman 7:59116528d895 28 float Kp1 = 1.5;
wbuysman 6:2887ad4c5d97 29 float Ki1 = 0.5;
wbuysman 2:302174acdac7 30 float error1 = 0;
wbuysman 2:302174acdac7 31 float output1 = 0;
wbuysman 6:2887ad4c5d97 32 float omrekenfactor1 = (32*70)/6.28;
wbuysman 6:2887ad4c5d97 33
wbuysman 6:2887ad4c5d97 34 float integral2 = 0;
wbuysman 6:2887ad4c5d97 35 float setpoint2 = 3.14;
wbuysman 7:59116528d895 36 float dt2 = 0.001;
wbuysman 6:2887ad4c5d97 37 float Kp2 = 0.55;
wbuysman 6:2887ad4c5d97 38 float Ki2 = 0.45;
wbuysman 6:2887ad4c5d97 39 float error2 = 0;
wbuysman 6:2887ad4c5d97 40 float output2 = 0;
wbuysman 6:2887ad4c5d97 41 float omrekenfactor2 = (24*172)/6.28;
wbuysman 3:e40763e01a80 42
wbuysman 7:59116528d895 43 float setpointrem1 = 0;
wbuysman 7:59116528d895 44 float integralrem1 = 0;
wbuysman 7:59116528d895 45 float Kprem1 = 1.5;
wbuysman 7:59116528d895 46 float Kirem1 = 0.5;
wbuysman 7:59116528d895 47 float errorrem1 = 0;
wbuysman 7:59116528d895 48 float outputrem1 = 0;
wbuysman 7:59116528d895 49
wbuysman 7:59116528d895 50
wbuysman 0:c205dc094877 51 while(1) {
wbuysman 7:59116528d895 52 error2 = setpoint2 - motor2.getPosition()/omrekenfactor2;
wbuysman 7:59116528d895 53 integral2 = integral2+ error2*dt2;
wbuysman 7:59116528d895 54 output2 = Kp2*error2 + Ki2*integral2;
wbuysman 7:59116528d895 55 keep_in_range(&output2,-1,1);
wbuysman 7:59116528d895 56 pwm_motor2.write(abs(output2));
wbuysman 7:59116528d895 57 if(output2 > 0) {
wbuysman 7:59116528d895 58 motor2dir = 1;
wbuysman 7:59116528d895 59 } else {
wbuysman 7:59116528d895 60 motor2dir = 0;
wbuysman 6:2887ad4c5d97 61 }
wbuysman 7:59116528d895 62 wait(dt2);
wbuysman 3:e40763e01a80 63
wbuysman 7:59116528d895 64 error1 = setpoint1 - motor1.getSpeed()/omrekenfactor1;
wbuysman 7:59116528d895 65 //motorpositie omgerekend naar rad/s
wbuysman 7:59116528d895 66 integral1 = integral1 + error1*dt1;
wbuysman 7:59116528d895 67 output1 = Kp1*error1 + Ki1*integral1;
wbuysman 7:59116528d895 68 keep_in_range(&output1,-1,1);
wbuysman 7:59116528d895 69 pwm_motor1.write(abs(output1));
wbuysman 3:e40763e01a80 70 if(output1 > 0) {
wbuysman 5:7fb05dfead4d 71 motor1dir = 1;
wbuysman 3:e40763e01a80 72 } else {
wbuysman 5:7fb05dfead4d 73 motor1dir = 0;
wbuysman 3:e40763e01a80 74 }
wbuysman 7:59116528d895 75 wait(dt1);
wbuysman 6:2887ad4c5d97 76
wbuysman 7:59116528d895 77 if(motor1.getPosition()/omrekenfactor1 < 1.3) {
wbuysman 7:59116528d895 78 setpoint1=8;
wbuysman 7:59116528d895 79 }
wbuysman 7:59116528d895 80
wbuysman 7:59116528d895 81 else {
wbuysman 7:59116528d895 82 setpoint1=0;
wbuysman 6:2887ad4c5d97 83
wbuysman 7:59116528d895 84 /*errorrem1 = setpointrem1 - motor1.getSpeed()/omrekenfactor1;
wbuysman 7:59116528d895 85 //motorpositie omgerekend naar rad/s
wbuysman 7:59116528d895 86 integralrem1 = integralrem1 + errorrem1*dt1;
wbuysman 7:59116528d895 87 outputrem1 = Kprem1*errorrem1 + Kirem1*integralrem1;
wbuysman 7:59116528d895 88 keep_in_range(&outputrem1,-1,1);
wbuysman 7:59116528d895 89 pwm_motor1.write(abs(outputrem1));
wbuysman 7:59116528d895 90 if(outputrem1 > 0) {
wbuysman 7:59116528d895 91 motor1dir = 1;
wbuysman 7:59116528d895 92 } else {
wbuysman 7:59116528d895 93 motor1dir = 0;
wbuysman 7:59116528d895 94 }
wbuysman 7:59116528d895 95 wait(dt1);*/
wbuysman 7:59116528d895 96 }
wbuysman 6:2887ad4c5d97 97
wbuysman 6:2887ad4c5d97 98 scope.set(0, output1);
wbuysman 6:2887ad4c5d97 99 scope.set(1, motor1.getSpeed()/omrekenfactor1);
wbuysman 6:2887ad4c5d97 100 scope.set(2, motor1.getPosition()/(omrekenfactor1));
wbuysman 6:2887ad4c5d97 101 scope.set(3, output2);
wbuysman 6:2887ad4c5d97 102 scope.set(4, motor2.getSpeed()/omrekenfactor2);
wbuysman 6:2887ad4c5d97 103 scope.set(5, motor2.getPosition()/(omrekenfactor2));
wbuysman 4:91b583d4d8c4 104 scope.send();
wbuysman 0:c205dc094877 105 }
wbuysman 5:7fb05dfead4d 106 }
wbuysman 5:7fb05dfead4d 107 void keep_in_range(float * in, float min, float max)
wbuysman 5:7fb05dfead4d 108 {
wbuysman 5:7fb05dfead4d 109 *in > min ? *in < max? : *in = max: *in = max;
wbuysman 0:c205dc094877 110 }