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@5:7fb05dfead4d, 2014-10-23 (annotated)

Committer:
wbuysman
Date:
Thu Oct 23 14:18:58 2014 +0000
Revision:
5:7fb05dfead4d
Parent:
4:91b583d4d8c4
Child:
6:2887ad4c5d97
werkende versie, richtingen omgedraaid;

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 4:91b583d4d8c4 6 HIDScope scope(4);
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 4:91b583d4d8c4 26 float setpoint1 = 3;
wbuysman 2:302174acdac7 27 float dt1 = 0.01;
wbuysman 4:91b583d4d8c4 28 float Kp1 = 1;
wbuysman 5:7fb05dfead4d 29 float Ki1 = 0.1;
wbuysman 2:302174acdac7 30 float error1 = 0;
wbuysman 2:302174acdac7 31 float output1 = 0;
wbuysman 4:91b583d4d8c4 32 float omrekenfactor = 4480.0/6.28;
wbuysman 3:e40763e01a80 33
wbuysman 0:c205dc094877 34 while(1) {
wbuysman 3:e40763e01a80 35
wbuysman 4:91b583d4d8c4 36 error1 = setpoint1 - motor1.getSpeed()/omrekenfactor; // (omrekenfactor)/dt1;
wbuysman 3:e40763e01a80 37 //motorpositie omgerekend naar rad/s
wbuysman 0:c205dc094877 38 integral1 = integral1 + error1*dt1;
wbuysman 0:c205dc094877 39 output1 = Kp1*error1 + Ki1*integral1;
wbuysman 5:7fb05dfead4d 40 keep_in_range(&output1,-1,1);
wbuysman 2:302174acdac7 41 pwm_motor1.write(abs(output1));
wbuysman 4:91b583d4d8c4 42 wait(dt1);
wbuysman 3:e40763e01a80 43
wbuysman 0:c205dc094877 44
wbuysman 3:e40763e01a80 45 if(output1 > 0) {
wbuysman 5:7fb05dfead4d 46 motor1dir = 1;
wbuysman 3:e40763e01a80 47 } else {
wbuysman 5:7fb05dfead4d 48 motor1dir = 0;
wbuysman 3:e40763e01a80 49 }
wbuysman 4:91b583d4d8c4 50 scope.set(0, error1);
wbuysman 4:91b583d4d8c4 51 scope.set(1, output1);
wbuysman 4:91b583d4d8c4 52 scope.set(2, motor1.getSpeed()/omrekenfactor);
wbuysman 4:91b583d4d8c4 53 scope.set(3, motor1.getPosition()/(omrekenfactor));
wbuysman 4:91b583d4d8c4 54 scope.send();
wbuysman 3:e40763e01a80 55
wbuysman 0:c205dc094877 56 }
wbuysman 5:7fb05dfead4d 57 }
wbuysman 5:7fb05dfead4d 58 void keep_in_range(float * in, float min, float max)
wbuysman 5:7fb05dfead4d 59 {
wbuysman 5:7fb05dfead4d 60 *in > min ? *in < max? : *in = max: *in = max;
wbuysman 0:c205dc094877 61 }