Wouter Bos / Mbed 2 deprecated Motor_potbesturing_notgradual

motor aansturing moet lineair zijn is het niet

Dependencies:   MODSERIAL Encoder mbed HIDScope

main.cpp@17:d643e5954165, 2015-10-07 (annotated)

Committer:
Zeekat
Date:
Wed Oct 07 12:33:19 2015 +0000
Revision:
17:d643e5954165
Parent:
16:acf850a87e01
Child:
18:e3b41351ee71
werkende versie met signaalvolging, Vmax = 5 rad/s. HIDScope is eruit want doet vreemd.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Zeekat 0:5ea1875b307a 1 #include "mbed.h"
Zeekat 17:d643e5954165 2 #include "MODSERIAL.h"
Zeekat 9:f907915f269c 3 #include "encoder.h"
Zeekat 17:d643e5954165 4 // #include "HIDScope.h"
Zeekat 4:e171c9fa5447 5
Zeekat 17:d643e5954165 6 Serial pc(USBTX,USBRX);
Zeekat 17:d643e5954165 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 16:acf850a87e01 42 // define storage variables for setpoint values
Zeekat 16:acf850a87e01 43 double c_setpoint1 = 0;
Zeekat 16:acf850a87e01 44 double c_setpoint2 = 0;
Zeekat 16:acf850a87e01 45
Zeekat 16:acf850a87e01 46 // define the maximum rate of change for the setpoint (velocity)
Zeekat 16:acf850a87e01 47 double Vmax = 5; // rad/sec
Zeekat 16:acf850a87e01 48
Zeekat 12:e3c5c5acbd09 49
Zeekat 17:d643e5954165 50
Zeekat 11:d31b03b05f59 51 // counts 2 radians
Zeekat 12:e3c5c5acbd09 52 // this function takes the counts from the encoder and converts it to
Zeekat 12:e3c5c5acbd09 53 // the amount of radians from the zero position.
Zeekat 12:e3c5c5acbd09 54 double get_radians(double counts)
Zeekat 0:5ea1875b307a 55 {
Zeekat 11:d31b03b05f59 56 double pi = 3.14159265359;
Zeekat 11:d31b03b05f59 57 double radians = (counts/4200)*2*pi;
Zeekat 11:d31b03b05f59 58 return radians;
Zeekat 0:5ea1875b307a 59 }
Zeekat 0:5ea1875b307a 60
Zeekat 12:e3c5c5acbd09 61
Zeekat 12:e3c5c5acbd09 62 // this function takes a 0->1 input (in this case a potmeter and converts it
Zeekat 12:e3c5c5acbd09 63 // to a -2->2 range
Zeekat 16:acf850a87e01 64 double setpoint_f(double input, double &c_setpoint)
Zeekat 0:5ea1875b307a 65 {
Zeekat 12:e3c5c5acbd09 66 double offset = 0.5; // offset the inputsignal to -0.5->0.5
Zeekat 16:acf850a87e01 67 double gain = 2; // increase the signal
Zeekat 16:acf850a87e01 68 double potset = (input-offset)*gain;
Zeekat 16:acf850a87e01 69 double setpoint = c_setpoint + potset * controlstep * Vmax ;
Zeekat 17:d643e5954165 70 c_setpoint = setpoint;
Zeekat 16:acf850a87e01 71 return setpoint;
Zeekat 11:d31b03b05f59 72 }
Zeekat 11:d31b03b05f59 73
Zeekat 12:e3c5c5acbd09 74 // this function is a simple K control called by the motor function
Zeekat 15:a90c450b1e0e 75 double K_control(double error,double K)
Zeekat 11:d31b03b05f59 76 {
Zeekat 12:e3c5c5acbd09 77 double output = K*error; // controller output K*e
Zeekat 14:92614abdb7e3 78
Zeekat 14:92614abdb7e3 79 // Limit the output to to a number between -1 and 1 because
Zeekat 14:92614abdb7e3 80 // the motorcode will not handle anything smaller than -1 or larger than 1
Zeekat 14:92614abdb7e3 81 // should be put in own function to improve readability
Zeekat 14:92614abdb7e3 82 if(output>1)
Zeekat 14:92614abdb7e3 83 {
Zeekat 14:92614abdb7e3 84 output = 1;
Zeekat 14:92614abdb7e3 85 }
Zeekat 14:92614abdb7e3 86 else if(output < 1 && output > 0)
Zeekat 14:92614abdb7e3 87 {
Zeekat 14:92614abdb7e3 88 output = output;
Zeekat 14:92614abdb7e3 89 }
Zeekat 14:92614abdb7e3 90 else if(output > -1 && output < 0)
Zeekat 14:92614abdb7e3 91 {
Zeekat 14:92614abdb7e3 92 output = output;
Zeekat 14:92614abdb7e3 93 }
Zeekat 14:92614abdb7e3 94 else if(output < -1)
Zeekat 14:92614abdb7e3 95 {
Zeekat 14:92614abdb7e3 96 (output = -1);
Zeekat 14:92614abdb7e3 97 }
Zeekat 11:d31b03b05f59 98 return output;
Zeekat 11:d31b03b05f59 99 }
Zeekat 11:d31b03b05f59 100
Zeekat 14:92614abdb7e3 101 // this function controls the input for one of the electric motors and is called by a ticker
Zeekat 15:a90c450b1e0e 102 // MOTOR 1
Zeekat 15:a90c450b1e0e 103 // // // // // // // // // //
Zeekat 11:d31b03b05f59 104 void motor1_control()
Zeekat 11:d31b03b05f59 105 {
Zeekat 17:d643e5954165 106 double setpoint1 = setpoint_f(potright.read(), c_setpoint1); // determine the setpoint that has been set by the inputsignal
Zeekat 15:a90c450b1e0e 107 double rads1 = get_radians(motor1_enc.getPosition()); // determine the position of the motor
Zeekat 15:a90c450b1e0e 108 double error1 = (setpoint1 - rads1); // determine the error (reference - position)
Zeekat 17:d643e5954165 109 // scope.set(0,setpoint1);
Zeekat 17:d643e5954165 110 // scope.set(1,rads1);
Zeekat 17:d643e5954165 111 // scope.send();
Zeekat 15:a90c450b1e0e 112 double output1 = K_control(error1, K1); // bereken de controller output voor motor 1(zie hierboven)
Zeekat 15:a90c450b1e0e 113 if(output1 > 0) { //
Zeekat 11:d31b03b05f59 114 motor1_rich.write(0);
Zeekat 15:a90c450b1e0e 115 motor1_aan.write(output1);
Zeekat 15:a90c450b1e0e 116 } else if(output1 < 0) {
Zeekat 11:d31b03b05f59 117 motor1_rich.write(1);
Zeekat 15:a90c450b1e0e 118 motor1_aan.write(abs(output1));
Zeekat 15:a90c450b1e0e 119 }
Zeekat 15:a90c450b1e0e 120 }
Zeekat 15:a90c450b1e0e 121
Zeekat 15:a90c450b1e0e 122 // MOTOR 2
Zeekat 15:a90c450b1e0e 123 // // // // // // //
Zeekat 15:a90c450b1e0e 124 void motor2_control()
Zeekat 15:a90c450b1e0e 125 {
Zeekat 17:d643e5954165 126 double setpoint2 = setpoint_f(potleft.read(), c_setpoint2); // determine the setpoint that has been set by the inputsignal
Zeekat 15:a90c450b1e0e 127 double rads2 = get_radians(motor2_enc.getPosition()); // determine the position of the motor
Zeekat 15:a90c450b1e0e 128 double error2 = (setpoint2 - rads2); // determine the error (reference - position)
Zeekat 17:d643e5954165 129 // scope.set(3,setpoint2);
Zeekat 17:d643e5954165 130 // scope.set(4,rads2);
Zeekat 17:d643e5954165 131 // scope.send();
Zeekat 15:a90c450b1e0e 132 double output2 = K_control(error2, K2); // bereken de controller output voor motor 1(zie hierboven)
Zeekat 15:a90c450b1e0e 133 if(output2 > 0) { //
Zeekat 15:a90c450b1e0e 134 motor2_rich.write(0);
Zeekat 15:a90c450b1e0e 135 motor2_aan.write(output2);
Zeekat 15:a90c450b1e0e 136 } else if(output2 < 0) {
Zeekat 15:a90c450b1e0e 137 motor2_rich.write(1);
Zeekat 15:a90c450b1e0e 138 motor2_aan.write(abs(output2));
Zeekat 11:d31b03b05f59 139 }
Zeekat 11:d31b03b05f59 140 }
Zeekat 4:e171c9fa5447 141
Zeekat 0:5ea1875b307a 142
Zeekat 0:5ea1875b307a 143 int main()
Zeekat 0:5ea1875b307a 144 {
Zeekat 15:a90c450b1e0e 145 controller1.attach(&motor1_control, controlstep);
Zeekat 15:a90c450b1e0e 146 controller2.attach(&motor2_control, controlstep);
Zeekat 11:d31b03b05f59 147 while(true)
Zeekat 0:5ea1875b307a 148 {
Zeekat 15:a90c450b1e0e 149 if(button.read() == button_pressed)
Zeekat 15:a90c450b1e0e 150 {
Zeekat 15:a90c450b1e0e 151 motor1_enc.setPosition(0);
Zeekat 15:a90c450b1e0e 152 motor2_enc.setPosition(0);
Zeekat 15:a90c450b1e0e 153 }
Zeekat 0:5ea1875b307a 154 }
Zeekat 0:5ea1875b307a 155 }