Wouter Bos / Mbed 2 deprecated Motor_potbesturing_notgradual

motor aansturing moet lineair zijn is het niet

Dependencies:   MODSERIAL Encoder mbed HIDScope

main.cpp@12:e3c5c5acbd09, 2015-10-05 (annotated)

Committer:
Zeekat
Date:
Mon Oct 05 11:01:30 2015 +0000
Revision:
12:e3c5c5acbd09
Parent:
11:d31b03b05f59
Child:
13:f6ecdd3f6db1
werkend met annotatie

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 12:e3c5c5acbd09 7 HIDScope scope(1); // definieerd het aantal kanalen van de scope
Zeekat 4:e171c9fa5447 8
Zeekat 12:e3c5c5acbd09 9 Ticker mod; // definieer de ticker die alles bijhoud.
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 12:e3c5c5acbd09 26
Zeekat 12:e3c5c5acbd09 27 // send specified data to hidscope
Zeekat 12:e3c5c5acbd09 28 void send()
Zeekat 12:e3c5c5acbd09 29 {
Zeekat 12:e3c5c5acbd09 30 scope.set(0,setpoint);
Zeekat 12:e3c5c5acbd09 31 scope.send();
Zeekat 12:e3c5c5acbd09 32 }
Zeekat 12:e3c5c5acbd09 33
Zeekat 11:d31b03b05f59 34 // counts 2 radians
Zeekat 12:e3c5c5acbd09 35 // this function takes the counts from the encoder and converts it to
Zeekat 12:e3c5c5acbd09 36 // the amount of radians from the zero position.
Zeekat 12:e3c5c5acbd09 37 double get_radians(double counts)
Zeekat 0:5ea1875b307a 38 {
Zeekat 11:d31b03b05f59 39 double pi = 3.14159265359;
Zeekat 11:d31b03b05f59 40 double radians = (counts/4200)*2*pi;
Zeekat 11:d31b03b05f59 41 return radians;
Zeekat 0:5ea1875b307a 42 }
Zeekat 0:5ea1875b307a 43
Zeekat 12:e3c5c5acbd09 44
Zeekat 12:e3c5c5acbd09 45 // this function takes a 0->1 input (in this case a potmeter and converts it
Zeekat 12:e3c5c5acbd09 46 // to a -2->2 range
Zeekat 11:d31b03b05f59 47 double setpoint_f(double input)
Zeekat 0:5ea1875b307a 48 {
Zeekat 12:e3c5c5acbd09 49 double offset = 0.5; // offset the inputsignal to -0.5->0.5
Zeekat 12:e3c5c5acbd09 50 double gain = 4; // increase the signal
Zeekat 11:d31b03b05f59 51 double output = (input-offset)*gain;
Zeekat 11:d31b03b05f59 52 return output;
Zeekat 11:d31b03b05f59 53 }
Zeekat 11:d31b03b05f59 54
Zeekat 12:e3c5c5acbd09 55 // this function is a simple K control called by the motor function
Zeekat 11:d31b03b05f59 56 double K_control()
Zeekat 11:d31b03b05f59 57 {
Zeekat 12:e3c5c5acbd09 58 double setpoint = setpoint_f(potright.read()); // determine the setpoint that has been set by the inputsignal
Zeekat 12:e3c5c5acbd09 59 double rads = get_radians(motor1_enc.getPosition()); // determine the position of the motor
Zeekat 12:e3c5c5acbd09 60 double error = (setpoint - rads); // determine the error (reference - position)
Zeekat 12:e3c5c5acbd09 61 double output = K*error; // controller output K*e
Zeekat 11:d31b03b05f59 62 return output;
Zeekat 11:d31b03b05f59 63 }
Zeekat 11:d31b03b05f59 64
Zeekat 12:e3c5c5acbd09 65 // this function controls the input for one of the electric motore and is called by a ticker
Zeekat 11:d31b03b05f59 66 void motor1_control()
Zeekat 11:d31b03b05f59 67 {
Zeekat 11:d31b03b05f59 68 double output = K_control();
Zeekat 11:d31b03b05f59 69 if(output > 0) {
Zeekat 11:d31b03b05f59 70 motor1_rich.write(0);
Zeekat 11:d31b03b05f59 71 motor1_aan.write(1);
Zeekat 11:d31b03b05f59 72 } else if(output < 0) {
Zeekat 11:d31b03b05f59 73 motor1_rich.write(1);
Zeekat 11:d31b03b05f59 74 motor1_aan.write(1);
Zeekat 11:d31b03b05f59 75 }
Zeekat 11:d31b03b05f59 76 }
Zeekat 4:e171c9fa5447 77
Zeekat 0:5ea1875b307a 78
Zeekat 0:5ea1875b307a 79 int main()
Zeekat 0:5ea1875b307a 80 {
Zeekat 11:d31b03b05f59 81 double output ;
Zeekat 11:d31b03b05f59 82 mod.attach(&send,0.01);
Zeekat 11:d31b03b05f59 83 mod.attach(&motor1_control, 0.1);
Zeekat 11:d31b03b05f59 84 while(true)
Zeekat 0:5ea1875b307a 85 {
Zeekat 0:5ea1875b307a 86 }
Zeekat 0:5ea1875b307a 87 }