Wouter Bos / Mbed 2 deprecated Motor_potbesturing_notgradual

motor aansturing moet lineair zijn is het niet

Dependencies:   MODSERIAL Encoder mbed HIDScope

main.cpp@16:acf850a87e01, 2015-10-07 (annotated)

Committer:
Zeekat
Date:
Wed Oct 07 12:07:37 2015 +0000
Revision:
16:acf850a87e01
Parent:
15:a90c450b1e0e
Child:
17:d643e5954165
niet werkende versie

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