BMT K9 groep 9 2015-2016
Goed werkende PID, vanaf nu dit script gebruiken.
Dependencies: Encoder HIDScope MODSERIAL mbed
Diff: main.cpp
- Revision:
- 0:50d492ea0fd0
- Child:
- 1:d9cfdc904b10
- Child:
- 3:eb5b57162e38
diff -r 000000000000 -r 50d492ea0fd0 main.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Thu Oct 15 07:57:23 2015 +0000
@@ -0,0 +1,370 @@
+#include "mbed.h"
+#include "encoder.h"
+#include "MODSERIAL.h"
+#include "math.h"
+#include "HIDScope.h"
+#define M_PI 3.14159265358979323846
+
+DigitalOut motor2direction(D4); //D4 en D5 zijn motor 2 (op het motorshield)
+PwmOut motor2speed(D5);
+DigitalOut motor1direction(D7); //D6 en D7 voor motor 1 (op het motorshield)
+PwmOut motor1speed(D6);
+AnalogIn potmeter1(A3); //blauw draaiknopje, nr1
+AnalogIn potmeter2(A2); //nr 2
+Encoder encoder2(D13,D12,true);
+Encoder encoder1(D11,D10,true);
+MODSERIAL pc(USBTX,USBRX);
+
+Ticker myControllerTicker1, myControllerTicker2, countStepTickerX, countStepTickerY, buttonStateCheckX, buttonStateCheckY;
+bool motor1_PID_Controller_go = false;
+bool countStepX_go = false;
+bool countStepY_go = false;
+bool checkButtonStateX_go = false;
+bool checkButtonStateY_go = false;
+
+DigitalIn telknopX(SW2);
+DigitalIn telknopY(SW3);
+DigitalOut ledX(D2);
+DigitalOut ledY(D3);
+HIDScope scope(2);
+
+const double motor1_Kp = 1; //4200=1 rondje
+double m1_Kp = 0.0005, m1_Ki = 0.0, m1_Kd = 0.0;
+const double m1_Ts = 0.01;
+double m1_err_int = 0, m1_prev_err = 0;
+double m1_u_prev = 0;
+
+const int grens = 50;
+
+/*
+Gain = 0.002356
+B = [ 1.00000000000, 2.00000000000, 1.00000000000]
+A = [ 1.00000000000, -1.85808607200, 0.86750940236]
+*/
+
+const double m1_f_a1 = -1.94042975320, m1_f_a2 = 0.94215346226, m1_f_b0 = 1.0*0.000431, m1_f_b1 = 2.0*0.000431, m1_f_b2 = 1.0*0.000431;
+ //dit zijn de constanten die bij de filter horen (de a0 t/m b2) a0 is altijd 1
+double m1_f_v1 = 0, m1_f_v2 = 0; //de variabelen van de filter
+
+int reference1;
+int reference2;
+int difference1;
+int difference2;
+int buttonStateX = 1;
+int buttonStateY = 1;
+int motorDirectionX = 1;
+int motorDirectionY = 1;
+int n;
+int countsX=0;
+int countsY=0;
+int countStateX = 1;
+int countStateY = 1;
+
+float x = 4.0; // voorbeeld, moet eigenlijk bepaald worden door EMG signaal
+float y = 4.0; // is het 4de vakje van rechtsonderin de hoek geteld
+float xdist_from_board = 4.0; //er moet 2 van afgetrokken worden. Als de as 6cm van het bord staat moet er 4 worden ingevuld. Telt namelijk
+float ydist_from_board = 4.0; //vanaf het midden van een vakje.
+double constante1;
+double constante2;
+double constante3;
+
+bool buttonReleasedX(DigitalIn& button)
+{
+ if (button.read() == 1 && buttonStateX == 0) {
+ buttonStateX = 1;
+ return true;
+ }
+ buttonStateX = button.read();
+ return false;
+}
+
+bool buttonReleasedY(DigitalIn& button)
+{
+ if (button.read() == 1 && buttonStateY == 0) {
+ buttonStateY = 1;
+ return true;
+ }
+ buttonStateY = button.read();
+ return false;
+}
+
+bool buttonHold(DigitalIn& button)
+{
+ return (button.read() == 0);
+}
+
+float bereken_hoek1(float x, float y)
+{
+ float langste_zijde = sqrt(pow(x*4,2) + pow(y*4,2)); //pythagoras
+ float alpha = acos((langste_zijde/2)/30);
+ return (atan((4*y)/(4*x)) + alpha)*4200/(2*M_PI); // omrekenen van radialen naar counts = *4200/(2*M_PI), dit moet nog omgerekend worden met de gear ratio van de extra tandwielen.
+}
+
+float bereken_hoek2(float x, float y)
+{
+ float langste_zijde = sqrt(pow(x*4,2) + pow(y*4,2)); //pythagoras
+ float alpha = acos((langste_zijde/2)/30);
+ float beta = 0.5*M_PI - alpha;
+ return (2*beta)*4200/(2*M_PI); // x wordt hoek voor eerste arm t.o.v. x-as (CW), y hoek voor tweede arm t.o.v. eerste arm (CW)
+}
+
+//-------------------------- MOTORCONTROLLERS --------------------------//
+
+void constanteBepalen()
+{
+ constante1 = potmeter1.read();
+ constante2 = potmeter2.read();
+ m1_Kd = constante2*0.001; //loopt nu van 0 tot 1
+ m1_Ki = constante1*0.001;
+}
+
+double P(double error, const double Kp) //P regelaar
+{
+ return Kp*error; //ingangssignaal voor de motor\plant) = dus voltage
+}
+
+double PI(double e, double Kp, const double Ki, double Ts, double &e_int)
+{
+ e_int = e_int + Ts * e; // e_int is changed globally because it’s ’by reference’ (&)
+ return Kp * e + Ki * e_int;
+}
+
+double biquad(double u, double &v1, double &v2, const double a1, const double a2,
+ const double b0, const double b1, const double b2){
+ double v = u-a1*v1-a2*v2; //formule uit slides van biquad filter
+ double y = b0*v+b1*v1+b2*v2; //zie ook slides
+ v2 = v1; v1 =v;
+ return y;
+}
+
+double PID(double e, const double Kp, const double Ki, const double Kd, double Ts,
+ double &e_int, double &e_prev, double &f_v1, double &f_v2, const double f_a1,
+ const double f_a2, const double f_b0, const double f_b1, const double f_b2){ //In de slides staat f_b3, maar volgens mij moet het f_b2 zijn
+ //Bereken eerst afgeleide van de error
+ double e_der = (e-e_prev)/Ts;
+ double e_der2 = biquad(e_der, f_v1, f_v2, f_a1, f_a2, f_b0, f_b1, f_b2); //de y die de biquad returnt is de gefilterde e_der
+ e_prev = e;
+ //Bereken nu de geintegreerde versie van de error
+ e_int = e_int + Ts*e;
+ //PID
+
+ return Kp*e+Ki*e_int+Kd*e_der2; //formule uit slides
+}
+
+// P controllers
+void motor2_P_Controller()
+{
+ reference2 = potmeter2.read()*4200; //draaiknop uitlezen, tussen 0 en 1
+ //reference2 = y;
+ double position =(encoder2.getPosition()); //waarde tussen 0 en 4200
+ double u = P(reference2 - position, motor1_Kp); //P controller aanroepen
+
+ if (u > 0) { //directie bepalen
+ motor2direction=1;
+ } else if (u <= 0) {
+ motor2direction =0;
+ }
+ if (u <grens && u>-grens) { //bepalen of de motor aan of uit moet
+ motor2speed = 0.0f;
+ } else {
+ motor2speed = 0.1f;
+ }
+}
+
+void motor1_P_Controller()
+{
+ reference1 = potmeter1.read()*4200; //draaiknop uitlezen, tussen 0 en 1
+ //reference1=x;
+ double position =(encoder1.getPosition()); // waarde tussen 0 en 4200
+ difference1 = reference1-position;
+ double u = P(reference1 - position, motor1_Kp); //P controller aanroepen
+ if (u > 0) { //directie bepalen. Waardes zijn tegenovergesteld vergeleken met motor 2
+ motor1direction=0;
+ } else if (u <= 0) {
+ motor1direction =1;
+ }
+ if (u <grens && u >-grens) { //bepalen of de motor aan of uit moet
+ motor1speed = 0.0f;
+ } else {
+ motor1speed = 0.1f;
+ }
+}
+
+//PI controllers
+void motor1_PI_Controller()
+{
+ //reference1 = potmeter1.read()*4200; //draaiknop uitlezen, tussen 0 en 1
+ float hoek1 = bereken_hoek1(countsX,countsY);
+ reference1=hoek1;
+ double position =(encoder1.getPosition()); // waarde tussen 0 en 4200
+
+ difference1 = reference1 - position;
+ constanteBepalen();
+ double u = PI(reference1 - position, m1_Kp, m1_Ki, m1_Ts, m1_err_int);
+ if (u > 0) { //directie bepalen. Waardes zijn tegenovergesteld vergeleken met motor 2
+ motor1direction=0;
+ } else if (u <= 0) {
+ motor1direction =1;
+ }
+ if (u <grens && u >-grens) { //bepalen of de motor aan of uit moet
+ motor1speed = 0.0f;
+ } else {
+ motor1speed = 0.1f;
+ }
+}
+
+void motor2_PI_Controller()
+{
+ //reference2 = potmeter2.read()*4200; //draaiknop uitlezen, tussen 0 en 1
+ float hoek2 = bereken_hoek2(countsX,countsY);
+ reference2 = hoek2;
+ double position =(encoder2.getPosition()); //waarde tussen 0 en 4200
+
+ difference2 = reference2 - position;
+ constanteBepalen();
+ double u = PI(reference2 - position, m1_Kp, m1_Ki, m1_Ts, m1_err_int);
+ if (u > 0) { //directie bepalen. Waardes zijn tegenovergesteld vergeleken met motor 2
+ motor2direction=1;
+ } else if (u <= 0) {
+ motor2direction =0;
+ }
+ if (u <grens && u >-grens) { //bepalen of de motor aan of uit moet
+ motor2speed = 0.0f;
+ } else {
+ motor2speed = 0.1f;
+ }
+}
+
+void motor1_PID_Controller()
+{
+ //reference1 = potmeter1.read()*4200; //draaiknop uitlezen, tussen 0 en 1
+ float hoek1 = bereken_hoek1(countsX,countsY);
+ reference1=hoek1;
+ double position =(encoder1.getPosition()); // waarde tussen 0 en 4200
+
+ scope.set(0,reference1);
+ scope.set(1,position);
+ scope.send();
+
+ difference1 = reference1 - position;
+ constanteBepalen();
+ double u = PID( reference1 - position, m1_Kp, m1_Ki, m1_Kd, m1_Ts, m1_err_int,m1_prev_err, m1_f_v1, m1_f_v2, m1_f_a1, m1_f_a2, m1_f_b0, m1_f_b1, m1_f_b2 );
+ if (u < 0) {
+ motor1direction = 1; //directie bepalen. Waardes zijn tegenovergesteld vergeleken met motor 2
+ } else if (u>=0) {
+ motor1direction = 0;
+ }
+ motor1speed = fabs(u);
+ pc.printf("u = %.2f \r\n", u);
+
+}
+
+//-------------------------- POSITIEBEPALING --------------------------//
+
+void checkButtonStateX()
+{
+ if (buttonReleasedX(telknopX)) {
+ countStateX = -countStateX;
+ }
+}
+
+void checkButtonStateY()
+{
+ if (buttonReleasedY(telknopY)) {
+ countStateY = -countStateY;
+ }
+}
+
+void countStepX()
+{
+ if (buttonHold(telknopX)) {
+ countsX+= countStateX; //hier moet de richting dus veranderen
+ if(countsX >= 10) {
+ countsX = 10;
+ } else if(countsX<=0) {
+ countsX = 0;
+ }
+ }
+}
+
+void countStepY()
+{
+ if (buttonHold(telknopY)) { //X en Y kunnen niet tegelijk optellen, alleen als de ander niet ingedrukt is
+ countsY+= countStateY;
+ if(countsY >= 10) {
+ countsY = 10;
+ } else if(countsY<=0) {
+ countsY = 0;
+ }
+ }
+}
+
+void motor1_PID_Controller_activate(){
+ motor1_PID_Controller_go = true;
+}
+
+void countStepX_activate(){
+ countStepX_go = true;
+}
+
+void checkButtonStateX_activate(){
+ checkButtonStateX_go = true;
+}
+
+void countStepY_activate(){
+ countStepY_go = true;
+}
+
+void checkButtonStateY_activate(){
+ checkButtonStateY_go = true;
+}
+
+int main()
+{
+ pc.baud(9600);
+ encoder1.setPosition(0);
+ encoder2.setPosition(0);
+ /*myControllerTicker1.attach( &motor1_PID_Controller, 0.005f ); //Ticker roept elke 0.01 s de motor2 controller aan. = 100Hz
+ myControllerTicker2.attach( &motor2_PI_Controller, 0.01f);
+ countStepTickerX.attach( &countStepX,1.0f );
+ countStepTickerY.attach( &countStepY,1.0f);
+ buttonStateCheckX.attach( &checkButtonStateX,0.001f);
+ buttonStateCheckY.attach( &checkButtonStateY,0.001f);
+ */
+ myControllerTicker1.attach( &motor1_PID_Controller_activate, 0.01f); // Function@ hz
+ countStepTickerX.attach( &countStepX_activate, 1.0f); // Function@ hz
+ countStepTickerY.attach( &countStepY_activate, 1.04f); // Function@ hz
+ buttonStateCheckX.attach( &checkButtonStateX_activate, 0.001f); // Function@ hz
+ buttonStateCheckY.attach( &checkButtonStateY_activate, 0.001f); // Function@ hz
+
+
+ while (true) {
+
+ pc.printf(" 1: pos: %d, ref: %d, dif: %d, Ki: %.5f, Kd: %.5f, countsX: %d, countsY: %d \r\n", encoder1.getPosition(), reference1,difference1,m1_Kd,m1_Ki,countsX,countsY);
+ //pc.printf(" 2: pos: %d, ref: %d, dif: %d \r\n", encoder2.getPosition(), reference2,difference2);
+
+ if(motor1_PID_Controller_go) {
+ motor1_PID_Controller_go = false;
+ motor1_PID_Controller();
+ }
+ if(countStepX_go) {
+ countStepX_go = false;
+ countStepX();
+ }
+ if(countStepY_go) {
+ countStepY_go = false;
+ countStepY();
+ }
+ if(checkButtonStateX_go) {
+ checkButtonStateX_go = false;
+ checkButtonStateX();
+ }
+ if(checkButtonStateY_go) {
+ checkButtonStateY_go = false;
+ checkButtonStateY();
+ }
+
+
+
+ }
+}
\ No newline at end of file