Wouter Bos / Mbed 2 deprecated Motor_potbesturing_notgradual

motor aansturing moet lineair zijn is het niet

Dependencies:   MODSERIAL Encoder mbed HIDScope

main.cpp@14:92614abdb7e3, 2015-10-06 (annotated)

Committer:
Zeekat
Date:
Tue Oct 06 12:56:33 2015 +0000
Revision:
14:92614abdb7e3
Parent:
13:f6ecdd3f6db1
Child:
15:a90c450b1e0e
P_controller done works good

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 13:f6ecdd3f6db1 7 HIDScope scope(2); // 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 11:d31b03b05f59 22 double setpoint;
Zeekat 13:f6ecdd3f6db1 23 double rads;
Zeekat 14:92614abdb7e3 24 const double K = 1 ;
Zeekat 11:d31b03b05f59 25
Zeekat 12:e3c5c5acbd09 26
Zeekat 11:d31b03b05f59 27 // counts 2 radians
Zeekat 12:e3c5c5acbd09 28 // this function takes the counts from the encoder and converts it to
Zeekat 12:e3c5c5acbd09 29 // the amount of radians from the zero position.
Zeekat 12:e3c5c5acbd09 30 double get_radians(double counts)
Zeekat 0:5ea1875b307a 31 {
Zeekat 11:d31b03b05f59 32 double pi = 3.14159265359;
Zeekat 11:d31b03b05f59 33 double radians = (counts/4200)*2*pi;
Zeekat 11:d31b03b05f59 34 return radians;
Zeekat 0:5ea1875b307a 35 }
Zeekat 0:5ea1875b307a 36
Zeekat 12:e3c5c5acbd09 37
Zeekat 12:e3c5c5acbd09 38 // this function takes a 0->1 input (in this case a potmeter and converts it
Zeekat 12:e3c5c5acbd09 39 // to a -2->2 range
Zeekat 11:d31b03b05f59 40 double setpoint_f(double input)
Zeekat 0:5ea1875b307a 41 {
Zeekat 12:e3c5c5acbd09 42 double offset = 0.5; // offset the inputsignal to -0.5->0.5
Zeekat 12:e3c5c5acbd09 43 double gain = 4; // increase the signal
Zeekat 11:d31b03b05f59 44 double output = (input-offset)*gain;
Zeekat 11:d31b03b05f59 45 return output;
Zeekat 11:d31b03b05f59 46 }
Zeekat 11:d31b03b05f59 47
Zeekat 12:e3c5c5acbd09 48 // this function is a simple K control called by the motor function
Zeekat 11:d31b03b05f59 49 double K_control()
Zeekat 11:d31b03b05f59 50 {
Zeekat 12:e3c5c5acbd09 51 double setpoint = setpoint_f(potright.read()); // determine the setpoint that has been set by the inputsignal
Zeekat 13:f6ecdd3f6db1 52 scope.set(0,setpoint);
Zeekat 12:e3c5c5acbd09 53 double rads = get_radians(motor1_enc.getPosition()); // determine the position of the motor
Zeekat 13:f6ecdd3f6db1 54 scope.set(1,rads);
Zeekat 13:f6ecdd3f6db1 55 scope.send();
Zeekat 12:e3c5c5acbd09 56 double error = (setpoint - rads); // determine the error (reference - position)
Zeekat 12:e3c5c5acbd09 57 double output = K*error; // controller output K*e
Zeekat 14:92614abdb7e3 58
Zeekat 14:92614abdb7e3 59 // Limit the output to to a number between -1 and 1 because
Zeekat 14:92614abdb7e3 60 // the motorcode will not handle anything smaller than -1 or larger than 1
Zeekat 14:92614abdb7e3 61 // should be put in own function to improve readability
Zeekat 14:92614abdb7e3 62 if(output>1)
Zeekat 14:92614abdb7e3 63 {
Zeekat 14:92614abdb7e3 64 output = 1;
Zeekat 14:92614abdb7e3 65 }
Zeekat 14:92614abdb7e3 66 else if(output < 1 && output > 0)
Zeekat 14:92614abdb7e3 67 {
Zeekat 14:92614abdb7e3 68 output = output;
Zeekat 14:92614abdb7e3 69 }
Zeekat 14:92614abdb7e3 70 else if(output > -1 && output < 0)
Zeekat 14:92614abdb7e3 71 {
Zeekat 14:92614abdb7e3 72 output = output;
Zeekat 14:92614abdb7e3 73 }
Zeekat 14:92614abdb7e3 74 else if(output < -1)
Zeekat 14:92614abdb7e3 75 {
Zeekat 14:92614abdb7e3 76 (output = -1);
Zeekat 14:92614abdb7e3 77 }
Zeekat 11:d31b03b05f59 78 return output;
Zeekat 11:d31b03b05f59 79 }
Zeekat 11:d31b03b05f59 80
Zeekat 14:92614abdb7e3 81 // this function controls the input for one of the electric motors and is called by a ticker
Zeekat 11:d31b03b05f59 82 void motor1_control()
Zeekat 11:d31b03b05f59 83 {
Zeekat 13:f6ecdd3f6db1 84 double output = K_control(); // bereken de controller output (zie hierboven)
Zeekat 13:f6ecdd3f6db1 85 if(output > 0) { //
Zeekat 11:d31b03b05f59 86 motor1_rich.write(0);
Zeekat 14:92614abdb7e3 87 motor1_aan.write(output);
Zeekat 11:d31b03b05f59 88 } else if(output < 0) {
Zeekat 11:d31b03b05f59 89 motor1_rich.write(1);
Zeekat 14:92614abdb7e3 90 motor1_aan.write(abs(output));
Zeekat 11:d31b03b05f59 91 }
Zeekat 11:d31b03b05f59 92 }
Zeekat 4:e171c9fa5447 93
Zeekat 0:5ea1875b307a 94
Zeekat 0:5ea1875b307a 95 int main()
Zeekat 0:5ea1875b307a 96 {
Zeekat 11:d31b03b05f59 97 mod.attach(&motor1_control, 0.1);
Zeekat 11:d31b03b05f59 98 while(true)
Zeekat 0:5ea1875b307a 99 {
Zeekat 0:5ea1875b307a 100 }
Zeekat 0:5ea1875b307a 101 }