Wouter Bos / Mbed 2 deprecated Motor_potbesturing_notgradual

motor aansturing moet lineair zijn is het niet

Dependencies:   MODSERIAL Encoder mbed HIDScope

main.cpp@11:d31b03b05f59, 2015-10-05 (annotated)

Committer:
Zeekat
Date:
Mon Oct 05 10:51:49 2015 +0000
Revision:
11:d31b03b05f59
Parent:
10:b2742f42de44
Child:
12:e3c5c5acbd09
werkende P-controller 1 motor

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Zeekat 0:5ea1875b307a 1 #include "mbed.h"
Zeekat 11:d31b03b05f59 2 // #include "MODSERIAL.h"
Zeekat 9:f907915f269c 3 #include "encoder.h"
Zeekat 10:b2742f42de44 4 #include "HIDScope.h"
Zeekat 4:e171c9fa5447 5
Zeekat 11:d31b03b05f59 6 // Serial pc(USBTX,USBRX);
Zeekat 11:d31b03b05f59 7 HIDScope scope(1);
Zeekat 4:e171c9fa5447 8
Zeekat 4:e171c9fa5447 9 Ticker mod;
Zeekat 0:5ea1875b307a 10
Zeekat 0:5ea1875b307a 11 //motor 1 gegevens
Zeekat 0:5ea1875b307a 12 PwmOut motor1_aan(D6); // PWM signaal motor 2 (uit sheets)
Zeekat 11:d31b03b05f59 13 DigitalOut motor1_rich(D7); // digitaal signaal voor richting
Zeekat 0:5ea1875b307a 14 // einde motor 1
Zeekat 0:5ea1875b307a 15
Zeekat 9:f907915f269c 16 // ENCODER
Zeekat 9:f907915f269c 17 Encoder motor1_enc(D12,D11);
Zeekat 0:5ea1875b307a 18
Zeekat 0:5ea1875b307a 19 //POTMETERS
Zeekat 0:5ea1875b307a 20 AnalogIn potright(A0);
Zeekat 0:5ea1875b307a 21
Zeekat 9:f907915f269c 22
Zeekat 11:d31b03b05f59 23 double setpoint;
Zeekat 11:d31b03b05f59 24 const double K = 2 ;
Zeekat 11:d31b03b05f59 25
Zeekat 11:d31b03b05f59 26 // counts 2 radians
Zeekat 11:d31b03b05f59 27 double get_radians(double counts)
Zeekat 0:5ea1875b307a 28 {
Zeekat 11:d31b03b05f59 29 double pi = 3.14159265359;
Zeekat 11:d31b03b05f59 30 double radians = (counts/4200)*2*pi;
Zeekat 11:d31b03b05f59 31 return radians;
Zeekat 0:5ea1875b307a 32 }
Zeekat 0:5ea1875b307a 33
Zeekat 11:d31b03b05f59 34 double setpoint_f(double input)
Zeekat 0:5ea1875b307a 35 {
Zeekat 11:d31b03b05f59 36 double offset = 0.5;
Zeekat 11:d31b03b05f59 37 double gain = 4;
Zeekat 11:d31b03b05f59 38 double output = (input-offset)*gain;
Zeekat 11:d31b03b05f59 39 return output;
Zeekat 11:d31b03b05f59 40 }
Zeekat 11:d31b03b05f59 41
Zeekat 11:d31b03b05f59 42 double K_control()
Zeekat 11:d31b03b05f59 43 {
Zeekat 11:d31b03b05f59 44 double setpoint = setpoint_f(potright.read());
Zeekat 11:d31b03b05f59 45 double rads = get_radians(motor1_enc.getPosition());
Zeekat 11:d31b03b05f59 46 double error = (setpoint - rads);
Zeekat 11:d31b03b05f59 47 double output = K*error;
Zeekat 11:d31b03b05f59 48 return output;
Zeekat 11:d31b03b05f59 49 }
Zeekat 11:d31b03b05f59 50
Zeekat 0:5ea1875b307a 51
Zeekat 4:e171c9fa5447 52 void send()
Zeekat 4:e171c9fa5447 53 {
Zeekat 11:d31b03b05f59 54 scope.set(0,setpoint);
Zeekat 11:d31b03b05f59 55 scope.send();
Zeekat 10:b2742f42de44 56 }
Zeekat 10:b2742f42de44 57
Zeekat 11:d31b03b05f59 58 void motor1_control()
Zeekat 11:d31b03b05f59 59 {
Zeekat 11:d31b03b05f59 60 double output = K_control();
Zeekat 11:d31b03b05f59 61 if(output > 0) {
Zeekat 11:d31b03b05f59 62 motor1_rich.write(0);
Zeekat 11:d31b03b05f59 63 motor1_aan.write(1);
Zeekat 11:d31b03b05f59 64 } else if(output < 0) {
Zeekat 11:d31b03b05f59 65 motor1_rich.write(1);
Zeekat 11:d31b03b05f59 66 motor1_aan.write(1);
Zeekat 11:d31b03b05f59 67 }
Zeekat 11:d31b03b05f59 68 }
Zeekat 4:e171c9fa5447 69
Zeekat 0:5ea1875b307a 70
Zeekat 0:5ea1875b307a 71 int main()
Zeekat 0:5ea1875b307a 72 {
Zeekat 11:d31b03b05f59 73 double output ;
Zeekat 11:d31b03b05f59 74 mod.attach(&send,0.01);
Zeekat 11:d31b03b05f59 75 mod.attach(&motor1_control, 0.1);
Zeekat 11:d31b03b05f59 76 while(true)
Zeekat 0:5ea1875b307a 77 {
Zeekat 0:5ea1875b307a 78 }
Zeekat 0:5ea1875b307a 79 }