first publish not working
Dependencies: MODSERIAL mbed ttmath FastPWM Motor_with_encoder biquadFilter
Diff: main.cpp
- Revision:
- 16:000f2ebbd16c
- Parent:
- 15:65f295a49a4b
- Child:
- 17:10c18ca3368b
diff -r 65f295a49a4b -r 000f2ebbd16c main.cpp --- a/main.cpp Thu Nov 02 17:43:05 2017 +0000 +++ b/main.cpp Mon Nov 06 22:50:16 2017 +0000 @@ -7,30 +7,34 @@ #include "BiQuad.h" - +//Reference signals DigitalOut gpo(D0); DigitalOut ledb(LED_BLUE); DigitalOut ledr(LED_RED); DigitalOut ledg(LED_GREEN); +//Motors DigitalOut motor1DC(D7); DigitalOut motor2DC(D4); FastPWM motor1PWM(D6); FastPWM motor2PWM(D5); +//Position control AnalogIn potMeter1(A0); AnalogIn potMeter2(A1); AnalogIn emg1(A2); AnalogIn emg2(A3); AnalogIn emg3(A4); AnalogIn emg4(A5); +Encoder Encoder1(D12,D13); +Encoder Encoder2(D8,D9); DigitalIn button1(D11); DigitalIn button2(D12); DigitalIn button3(SW2); DigitalIn button4(SW3); -Encoder Encoder1(D12,D13); -Encoder Encoder2(D8,D9); + + MODSERIAL pc(USBTX,USBRX); @@ -52,16 +56,16 @@ const double pi = 3.1415926535897; float Pwmperiod = 0.001f; int potmultiplier = 600; // Multiplier for the pot meter reference which is normally between 0 and 1 -const double gainM1 = 1/29.17; // encoder pulses per degree theta -const double gainM2 = 1/105.0; // pulses per mm +const double gainM1 = 1/29.17; // 1 / encoder pulses per degree theta +const double gainM2 = 1/105.0; // 1 / pulses per mm double motor1; double motor2; double reference_motor1 = 0; // reference for Theta double reference_motor2 = 0; -double pos_M1 = 0; // start angle theta -double pos_M2 = 0; // start radius +double pos_M1 = 0; // start angle theta +double pos_M2 = 0; // start radius //Start constants PID ------------------------------- @@ -170,11 +174,11 @@ double RA = in1+in2; - if (RA < 0.5) + if (RA < 0.8) { X = X; } - else if (RA > 1.5) + else if (RA > 1.8) { X = X-0.4; } @@ -216,7 +220,7 @@ if (potMeter2 < 0.3) { - Y = Y-0.5; + Y = Y-0.8; } else if (potMeter2 > 0.7) { @@ -235,17 +239,29 @@ double LA = in3+in4; - if (LA < 0.5) + if (LA < 0.8) { Y = Y; + + ledb = 1; + ledr = 1; + ledg = 0; //Groen } - else if (LA > 1.5) - { + else if (LA > 1.8) + { Y = Y-0.4; + + ledr = 1; + ledg = 1; //Blau + ledb = 0; } else { Y = Y+0.4; + + ledb = 1; //Rood + ledr = 0; + ledg = 1; } // */ if (Y >= Y_maxTreshold){ @@ -345,26 +361,13 @@ //motor2PWM = motor2; if(delta1 > 0.5){ - motor1DC = 0; - - ledr = 1; - ledg = 1; //Blau - ledb = 0; + motor1DC = 0; } else if (delta1< -0.5) { motor1DC = 1; - - ledb = 1; - ledr = 1; - ledg = 0; //Groen - } else{ motor1PWM = 0; - - ledb = 1; //Rood - ledr = 0; - ledg = 1; } motor1 = abs(delta1)/1000.0; @@ -376,25 +379,12 @@ if(delta2 > 2.0){ motor2DC = 0; - - ledr = 1; - ledg = 1; //Blau - ledb = 0; } else if (delta2<-2.0) { - motor2DC = 1; - - ledb = 1; - ledr = 1; - ledg = 0; //Groen - + motor2DC = 1; } else{ motor2PWM = 0; - - ledb = 1; //Rood - ledr = 0; - ledg = 1; } motor2 = abs(delta2)/1000.0; @@ -416,6 +406,10 @@ motor1PWM.period(Pwmperiod); motor2PWM.period(Pwmperiod); + ledr = 1; + ledb = 1; + ledg = 1; + bqc.add(&bq1).add(&bq2); while(1){