Wouter Bos / Mbed 2 deprecated Motor_potbesturing_notgradual

motor aansturing moet lineair zijn is het niet

Dependencies:   MODSERIAL Encoder mbed HIDScope

main.cpp@13:f6ecdd3f6db1, 2015-10-05 (annotated)

Committer:
Zeekat
Date:
Mon Oct 05 11:56:30 2015 +0000
Revision:
13:f6ecdd3f6db1
Parent:
12:e3c5c5acbd09
Child:
14:92614abdb7e3
last P version;

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 11:d31b03b05f59 24 const double K = 2 ;
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 11:d31b03b05f59 58 return output;
Zeekat 11:d31b03b05f59 59 }
Zeekat 11:d31b03b05f59 60
Zeekat 12:e3c5c5acbd09 61 // this function controls the input for one of the electric motore and is called by a ticker
Zeekat 11:d31b03b05f59 62 void motor1_control()
Zeekat 11:d31b03b05f59 63 {
Zeekat 13:f6ecdd3f6db1 64 double output = K_control(); // bereken de controller output (zie hierboven)
Zeekat 13:f6ecdd3f6db1 65 if(output > 0) { //
Zeekat 11:d31b03b05f59 66 motor1_rich.write(0);
Zeekat 11:d31b03b05f59 67 motor1_aan.write(1);
Zeekat 11:d31b03b05f59 68 } else if(output < 0) {
Zeekat 11:d31b03b05f59 69 motor1_rich.write(1);
Zeekat 11:d31b03b05f59 70 motor1_aan.write(1);
Zeekat 11:d31b03b05f59 71 }
Zeekat 11:d31b03b05f59 72 }
Zeekat 4:e171c9fa5447 73
Zeekat 13:f6ecdd3f6db1 74 // send specified data to hidscope
Zeekat 13:f6ecdd3f6db1 75 // word nu in control loop gedaan
Zeekat 13:f6ecdd3f6db1 76 void send()
Zeekat 13:f6ecdd3f6db1 77 {
Zeekat 13:f6ecdd3f6db1 78 scope.set(0,setpoint);
Zeekat 13:f6ecdd3f6db1 79 scope.set(1,rads);
Zeekat 13:f6ecdd3f6db1 80 scope.send();
Zeekat 13:f6ecdd3f6db1 81 }
Zeekat 0:5ea1875b307a 82
Zeekat 0:5ea1875b307a 83 int main()
Zeekat 0:5ea1875b307a 84 {
Zeekat 13:f6ecdd3f6db1 85 // mod.attach(&send,0.01); // send a signal to hidscope
Zeekat 11:d31b03b05f59 86 mod.attach(&motor1_control, 0.1);
Zeekat 11:d31b03b05f59 87 while(true)
Zeekat 0:5ea1875b307a 88 {
Zeekat 0:5ea1875b307a 89 }
Zeekat 0:5ea1875b307a 90 }