Wouter Bos / Mbed 2 deprecated Motor_potbesturing_notgradual

motor aansturing moet lineair zijn is het niet

Dependencies:   MODSERIAL Encoder mbed HIDScope

main.cpp@15:a90c450b1e0e, 2015-10-06 (annotated)

Committer:
Zeekat
Date:
Tue Oct 06 18:12:47 2015 +0000
Revision:
15:a90c450b1e0e
Parent:
14:92614abdb7e3
Child:
16:acf850a87e01
P-controller op apart beide motoren (nog niet getest!). verbeterpunt is aanpassen van de setpoint code om bereik v/d setpoint aan buiten de functie te zetten.

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 15:a90c450b1e0e 7 HIDScope scope(4); // definieerd het aantal kanalen van de scope
Zeekat 15:a90c450b1e0e 8
Zeekat 15:a90c450b1e0e 9 Ticker controller1, controller2; // definieer de ticker die controler1 doet
Zeekat 4:e171c9fa5447 10
Zeekat 0:5ea1875b307a 11
Zeekat 15:a90c450b1e0e 12 //MOTOR INPUTPINS
Zeekat 15:a90c450b1e0e 13 // motor 1
Zeekat 0:5ea1875b307a 14 PwmOut motor1_aan(D6); // PWM signaal motor 2 (uit sheets)
Zeekat 11:d31b03b05f59 15 DigitalOut motor1_rich(D7); // digitaal signaal voor richting
Zeekat 15:a90c450b1e0e 16
Zeekat 15:a90c450b1e0e 17 // motor 2
Zeekat 15:a90c450b1e0e 18 PwmOut motor2_aan(D5);
Zeekat 15:a90c450b1e0e 19 DigitalOut motor2_rich(D4);
Zeekat 15:a90c450b1e0e 20 // EINDE MOTOR
Zeekat 15:a90c450b1e0e 21
Zeekat 0:5ea1875b307a 22
Zeekat 9:f907915f269c 23 // ENCODER
Zeekat 9:f907915f269c 24 Encoder motor1_enc(D12,D11);
Zeekat 15:a90c450b1e0e 25 Encoder motor2_enc(D10,D9);
Zeekat 0:5ea1875b307a 26
Zeekat 0:5ea1875b307a 27 //POTMETERS
Zeekat 0:5ea1875b307a 28 AnalogIn potright(A0);
Zeekat 15:a90c450b1e0e 29 AnalogIn potleft(A1);
Zeekat 0:5ea1875b307a 30
Zeekat 15:a90c450b1e0e 31 // RESETBUTTON
Zeekat 15:a90c450b1e0e 32 DigitalIn button(PTA4);
Zeekat 15:a90c450b1e0e 33 int button_pressed = 0;
Zeekat 15:a90c450b1e0e 34
Zeekat 15:a90c450b1e0e 35
Zeekat 15:a90c450b1e0e 36 // controller stuff
Zeekat 15:a90c450b1e0e 37 double controlfreq = 10 ; // controlloops frequentie (Hz)
Zeekat 15:a90c450b1e0e 38 double controlstep = 1/controlfreq; // timestep derived from controlfreq
Zeekat 15:a90c450b1e0e 39 const double K1 = 1 ; // P constant motorcontrolle 1
Zeekat 15:a90c450b1e0e 40 const double K2 = 1; // p constant motorcontroller 2
Zeekat 11:d31b03b05f59 41
Zeekat 12:e3c5c5acbd09 42
Zeekat 11:d31b03b05f59 43 // counts 2 radians
Zeekat 12:e3c5c5acbd09 44 // this function takes the counts from the encoder and converts it to
Zeekat 12:e3c5c5acbd09 45 // the amount of radians from the zero position.
Zeekat 12:e3c5c5acbd09 46 double get_radians(double counts)
Zeekat 0:5ea1875b307a 47 {
Zeekat 11:d31b03b05f59 48 double pi = 3.14159265359;
Zeekat 11:d31b03b05f59 49 double radians = (counts/4200)*2*pi;
Zeekat 11:d31b03b05f59 50 return radians;
Zeekat 0:5ea1875b307a 51 }
Zeekat 0:5ea1875b307a 52
Zeekat 12:e3c5c5acbd09 53
Zeekat 12:e3c5c5acbd09 54 // this function takes a 0->1 input (in this case a potmeter and converts it
Zeekat 12:e3c5c5acbd09 55 // to a -2->2 range
Zeekat 11:d31b03b05f59 56 double setpoint_f(double input)
Zeekat 0:5ea1875b307a 57 {
Zeekat 12:e3c5c5acbd09 58 double offset = 0.5; // offset the inputsignal to -0.5->0.5
Zeekat 12:e3c5c5acbd09 59 double gain = 4; // increase the signal
Zeekat 11:d31b03b05f59 60 double output = (input-offset)*gain;
Zeekat 11:d31b03b05f59 61 return output;
Zeekat 11:d31b03b05f59 62 }
Zeekat 11:d31b03b05f59 63
Zeekat 12:e3c5c5acbd09 64 // this function is a simple K control called by the motor function
Zeekat 15:a90c450b1e0e 65 double K_control(double error,double K)
Zeekat 11:d31b03b05f59 66 {
Zeekat 12:e3c5c5acbd09 67 double output = K*error; // controller output K*e
Zeekat 14:92614abdb7e3 68
Zeekat 14:92614abdb7e3 69 // Limit the output to to a number between -1 and 1 because
Zeekat 14:92614abdb7e3 70 // the motorcode will not handle anything smaller than -1 or larger than 1
Zeekat 14:92614abdb7e3 71 // should be put in own function to improve readability
Zeekat 14:92614abdb7e3 72 if(output>1)
Zeekat 14:92614abdb7e3 73 {
Zeekat 14:92614abdb7e3 74 output = 1;
Zeekat 14:92614abdb7e3 75 }
Zeekat 14:92614abdb7e3 76 else if(output < 1 && output > 0)
Zeekat 14:92614abdb7e3 77 {
Zeekat 14:92614abdb7e3 78 output = output;
Zeekat 14:92614abdb7e3 79 }
Zeekat 14:92614abdb7e3 80 else if(output > -1 && output < 0)
Zeekat 14:92614abdb7e3 81 {
Zeekat 14:92614abdb7e3 82 output = output;
Zeekat 14:92614abdb7e3 83 }
Zeekat 14:92614abdb7e3 84 else if(output < -1)
Zeekat 14:92614abdb7e3 85 {
Zeekat 14:92614abdb7e3 86 (output = -1);
Zeekat 14:92614abdb7e3 87 }
Zeekat 11:d31b03b05f59 88 return output;
Zeekat 11:d31b03b05f59 89 }
Zeekat 11:d31b03b05f59 90
Zeekat 14:92614abdb7e3 91 // this function controls the input for one of the electric motors and is called by a ticker
Zeekat 15:a90c450b1e0e 92 // MOTOR 1
Zeekat 15:a90c450b1e0e 93 // // // // // // // // // //
Zeekat 11:d31b03b05f59 94 void motor1_control()
Zeekat 11:d31b03b05f59 95 {
Zeekat 15:a90c450b1e0e 96 double setpoint1 = setpoint_f(potright.read()); // determine the setpoint that has been set by the inputsignal
Zeekat 15:a90c450b1e0e 97 double rads1 = get_radians(motor1_enc.getPosition()); // determine the position of the motor
Zeekat 15:a90c450b1e0e 98 double error1 = (setpoint1 - rads1); // determine the error (reference - position)
Zeekat 15:a90c450b1e0e 99 scope.set(0,setpoint1);
Zeekat 15:a90c450b1e0e 100 scope.set(1,rads1);
Zeekat 15:a90c450b1e0e 101 scope.send();
Zeekat 15:a90c450b1e0e 102 double output1 = K_control(error1, K1); // bereken de controller output voor motor 1(zie hierboven)
Zeekat 15:a90c450b1e0e 103 if(output1 > 0) { //
Zeekat 11:d31b03b05f59 104 motor1_rich.write(0);
Zeekat 15:a90c450b1e0e 105 motor1_aan.write(output1);
Zeekat 15:a90c450b1e0e 106 } else if(output1 < 0) {
Zeekat 11:d31b03b05f59 107 motor1_rich.write(1);
Zeekat 15:a90c450b1e0e 108 motor1_aan.write(abs(output1));
Zeekat 15:a90c450b1e0e 109 }
Zeekat 15:a90c450b1e0e 110 }
Zeekat 15:a90c450b1e0e 111
Zeekat 15:a90c450b1e0e 112 // MOTOR 2
Zeekat 15:a90c450b1e0e 113 // // // // // // //
Zeekat 15:a90c450b1e0e 114 void motor2_control()
Zeekat 15:a90c450b1e0e 115 {
Zeekat 15:a90c450b1e0e 116 double setpoint2 = setpoint_f(potleft.read()); // determine the setpoint that has been set by the inputsignal
Zeekat 15:a90c450b1e0e 117 double rads2 = get_radians(motor2_enc.getPosition()); // determine the position of the motor
Zeekat 15:a90c450b1e0e 118 double error2 = (setpoint2 - rads2); // determine the error (reference - position)
Zeekat 15:a90c450b1e0e 119 scope.set(3,setpoint2);
Zeekat 15:a90c450b1e0e 120 scope.set(4,rads2);
Zeekat 15:a90c450b1e0e 121 scope.send();
Zeekat 15:a90c450b1e0e 122 double output2 = K_control(error2, K2); // bereken de controller output voor motor 1(zie hierboven)
Zeekat 15:a90c450b1e0e 123 if(output2 > 0) { //
Zeekat 15:a90c450b1e0e 124 motor2_rich.write(0);
Zeekat 15:a90c450b1e0e 125 motor2_aan.write(output2);
Zeekat 15:a90c450b1e0e 126 } else if(output2 < 0) {
Zeekat 15:a90c450b1e0e 127 motor2_rich.write(1);
Zeekat 15:a90c450b1e0e 128 motor2_aan.write(abs(output2));
Zeekat 11:d31b03b05f59 129 }
Zeekat 11:d31b03b05f59 130 }
Zeekat 4:e171c9fa5447 131
Zeekat 0:5ea1875b307a 132
Zeekat 0:5ea1875b307a 133 int main()
Zeekat 0:5ea1875b307a 134 {
Zeekat 15:a90c450b1e0e 135 controller1.attach(&motor1_control, controlstep);
Zeekat 15:a90c450b1e0e 136 controller2.attach(&motor2_control, controlstep);
Zeekat 11:d31b03b05f59 137 while(true)
Zeekat 0:5ea1875b307a 138 {
Zeekat 15:a90c450b1e0e 139 if(button.read() == button_pressed)
Zeekat 15:a90c450b1e0e 140 {
Zeekat 15:a90c450b1e0e 141 motor1_enc.setPosition(0);
Zeekat 15:a90c450b1e0e 142 motor2_enc.setPosition(0);
Zeekat 15:a90c450b1e0e 143 }
Zeekat 0:5ea1875b307a 144 }
Zeekat 0:5ea1875b307a 145 }