Janine Klein Rot
2014-10-15 Arm 1 naar thuispositie. Eerste poging, fout in keep_in_range
Dependencies: Encoder MODSERIAL TextLCD mbed mbed-dsp
Diff: main.cpp
- Revision:
- 20:1cb0cf0d49ac
- Parent:
- 19:38c9d177b6ee
- Child:
- 21:f4e9f6c28de1
--- a/main.cpp Thu Oct 30 16:12:46 2014 +0000
+++ b/main.cpp Thu Oct 30 19:29:31 2014 +0000
@@ -6,9 +6,9 @@
//Constanten definiëren en waarde geven
#define SAMPLETIME_REGELAAR 0.005 //Sampletijd ticker regelaar motor
-#define KP_arm1 0.5 //Factor proprotionele regelaar arm 1
-#define KI_arm1 0.1 //Factor integraal regelaar arm 1
-#define KD_arm1 0.01 //Factor afgeleide regelaar arm 1
+#define KP_arm1 0.02 //Factor proprotionele regelaar arm 1
+#define KI_arm1 0 //Factor integraal regelaar arm 1
+#define KD_arm1 0 //Factor afgeleide regelaar arm 1
#define KP_arm2 0.3 //Factor proprotionele regelaar arm 2
#define KI_arm2 0.1 //Factor integraal regelaar arm 2
#define KD_arm2 0.1 //Factor afgeleide regelaar arm 2
@@ -58,6 +58,8 @@
Ticker ticker_EMG; //Ticker voor EMG meten
Timer biceps_kalibratie; //Timer voor kalibratiemeting EMG biceps
Timer triceps_kalibratie; //Timer voor kalibratiemeting EMG triceps
+Ticker ticker_motor_arm1_pid;
+Ticker ticker_motor_arm2_pid;
//States definiëren
enum pipostate {RUST, KALIBRATIE_ARM1, KALIBRATIE_ARM2, EMG_KALIBRATIE_BICEPS, EMG_KALIBRATIE_TRICEPS, START, B, T, BB, BT, TB, TT, BBB, BBT, BTB, BTT, TBB, TBT, TTB, TTT, BBBB, BBBT, BBTB, BBTT, BTBB, BTBT, BTTB, BTTT, TBBB, TBBT, TBTB, TBTT, TTBB, TTBT, TTTB, TTTT}; //Alle states benoemen, ieder krijgt een getal beginnend met 0
@@ -137,35 +139,43 @@
}
}
-void arm1_naar_thuispositie() //Brengt arm 1 naar de beginpositie
+void motor_arm1_pid()
{
- referentie_arm1 = referentie_arm1 + 180.0/200.0;
-
- if (referentie_arm1 >= 180) { //Als het verschil tussen de voorgestelde en huidige positie nul is wordt de while loop gestopt (condities kloppen niet meer)
- referentie_arm1 = 180;
-
- }
error_arm1_kalibratie = (referentie_arm1 - puls_motor_arm1.getPosition()); //PWM naar motor is het verschil tussen het voorgestelde aantal pulsen en de huidige positie maal een factor
- integraal_arm1 = integraal_arm1 + error_arm1_kalibratie*SAMPLETIME_REGELAAR; //Integraal deel van regelaar
- afgeleide_arm1 = (error_arm1_kalibratie - vorige_error_arm1)/SAMPLETIME_REGELAAR; //Afgeleide deel van regelaar
- pwm_to_motor_arm1 = error_arm1_kalibratie*KP_arm1 + integraal_arm1*KI_arm1 + afgeleide_arm1*KD_arm1;//Output naar motor na PID regelaar
- keep_in_range(&pwm_to_motor_arm1, -1, 1);
+ integraal_arm1 = integraal_arm1 + error_arm1_kalibratie*SAMPLETIME_REGELAAR; //Integraal deel van regelaar
+ afgeleide_arm1 = (error_arm1_kalibratie - vorige_error_arm1)/SAMPLETIME_REGELAAR; //Afgeleide deel van regelaar
+ pwm_to_motor_arm1 = error_arm1_kalibratie*KP_arm1 + integraal_arm1*KI_arm1 + afgeleide_arm1*KD_arm1;//Output naar motor na PID regelaar
+ keep_in_range(&pwm_to_motor_arm1, -1, 1);
- if (pwm_to_motor_arm1 > 0) { //Als PWM is positief, dan richting 1
+ if (pwm_to_motor_arm1 > 0) //Als PWM is positief, dan richting 1
+ {
dir_motor_arm1.write(1);
- } else { //Anders richting nul
+ }
+ else //Anders richting nul
+ {
dir_motor_arm1.write(0);
}
pwm_motor_arm1.write(fabs(pwm_to_motor_arm1)); //Output PWM naar motor is de absolute waarde van de berekende PWM
- pc.printf("pulsmotorgetposition %d ", puls_motor_arm1.getPosition()); //Huidig aantal getelde pulsen van de encoder naar pc sturen
- pc.printf("pwmmotor %f\n\r", pwm_to_motor_arm1); //Huidige PWM waarde naar motor naar pc sturen
+}
- state = KALIBRATIE_ARM2; //State wordt kalibratie arm 2, zo daar naar volgende onderdeel
- pc.printf("KALIBRATIE_ARM1 afgerond\n"); //Tekst voor controle Arm 1 naar thuispositie
+void motor_arm2_pid()
+{
+ error_arm2_kalibratie = (referentie_arm2 - puls_motor_arm2.getPosition()); //PWM naar motor is het verschil tussen het voorgestelde aantal pulsen en de huidige positie maal een factor
+ integraal_arm2 = integraal_arm2 + error_arm2_kalibratie*SAMPLETIME_REGELAAR; //Integraal deel van regelaar
+ afgeleide_arm2 = (error_arm2_kalibratie - vorige_error_arm2)/SAMPLETIME_REGELAAR; //Afgeleide deel van regelaar
+ pwm_to_motor_arm2 = error_arm2_kalibratie*KP_arm2 + integraal_arm2*KI_arm2 + afgeleide_arm2*KD_arm2;//Output naar motor na PID regelaar
+ keep_in_range(&pwm_to_motor_arm2, -1, 1); //PWM moet tussen 1 en -1 blijven (niet meer dan 100% dutycycle)
+ if (pwm_to_motor_arm2 > 0) { //Als PWM is positief, dan richting 1
+ dir_motor_arm2.write(1);
+ } else { //Anders richting nul
+ dir_motor_arm2.write(0);
+ }
+ pwm_motor_arm2.write(fabs(pwm_to_motor_arm2)); //Output PWM naar motor is de absolute waarde van de berekende PWM
}
+
void arm2_naar_thuispositie() //Brengt arm 2 naar beginpositie
{
referentie_arm2 = referentie_arm2 + 35/200;
@@ -219,9 +229,11 @@
int main() //Main script
{
+ ticker_motor_arm1_pid.attach(motor_arm1_pid,SAMPLETIME_REGELAAR);
+ ticker_motor_arm2_pid.attach(motor_arm2_pid,SAMPLETIME_REGELAAR);
+
while(1) { //Oneindige while loop, anders wordt er na het uitvoeren van de eerste case niets meer gedaan
pc.baud(38400); //PC baud rate is 38400 bits/seconde
-
switch(state) { //Switch benoemen, zorgt ervoor dat in de goede volgorde de dingen worden doorlopen, aan einde een case wordt de state de naam van de nieuwe case
@@ -248,7 +260,18 @@
while(regelaar_ticker_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
- arm1_naar_thuispositie(); //Voer acties uit om arm 1 naar thuispositie te krijgen
+ referentie_arm1 = referentie_arm1 + 180.0/200.0;
+
+ pc.printf("pulsmotorgetposition1 %d ", puls_motor_arm1.getPosition()); //Huidig aantal getelde pulsen van de encoder naar pc sturen
+ pc.printf("pwmmotor1 %f ", pwm_to_motor_arm1); //Huidige PWM waarde naar motor naar pc sturen
+ pc.printf("referentie1 %f\n\r", referentie_arm1);
+
+ if (referentie_arm1 >= 180)
+ {
+ referentie_arm1 = 180;
+ state = KALIBRATIE_ARM2;
+ }
+
}
wait(1); //Een seconde wachten
break; //Stopt acties in deze case
@@ -264,7 +287,18 @@
while(regelaar_ticker_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
regelaar_ticker_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
- arm2_naar_thuispositie(); //Voer acties uit om arm 2 naar thuispositie te krijgen
+ referentie_arm2 = referentie_arm2 + 35/200;
+
+ pc.printf("pulsmotorgetposition2 %d ", puls_motor_arm2.getPosition()); //Huidig aantal getelde pulsen van de encoder naar pc sturen
+ pc.printf("pwmmotor2 %f ", pwm_to_motor_arm2); //Huidige PWM waarde naar motor naar pc sturen
+ pc.printf("referentie2 %f\n\r", referentie_arm2);
+
+ if(referentie_arm2 >= 35)
+ {
+ referentie_arm2 = 35;
+ state = EMG_KALIBRATIE_BICEPS;
+ }
+
}
wait(1); //Een seconde wachten
ticker_regelaar.detach(); //Ticker detachten, ticker doet nu niks meer
@@ -978,4 +1012,5 @@
}
//PWM moet tussen 1 en -1 blijven (niet meer dan 100% dutycycle)
}
- }
\ No newline at end of file
+ }
+}
\ No newline at end of file