Menno Sytsma
Dit is het passieve stuurprogramma van de handorthese behorend bij mijn bacheloropdracht. Groet, Menno Sytsma
Dependencies: EMG FastPWM HIDScope mbed-src
Diff: main.cpp
- Revision:
- 1:f63d8a73460c
- Parent:
- 0:46b6258b2b00
- Child:
- 2:1dd9e630a7b5
--- a/main.cpp Mon May 30 08:50:13 2016 +0000
+++ b/main.cpp Mon May 30 13:14:25 2016 +0000
@@ -40,6 +40,8 @@
double Error_prev = 0;
double Error_int = 0;
double Error_der;
+double Ref_der = 0;
+double Ref_prev = 0;
// Define the HIDScope and Ticker object
HIDScope scope(4);
Ticker Hidscope;
@@ -74,7 +76,7 @@
{
//double Inp = 10*B; // De potmeter geeft ook waardes tussen 0 en 1, dit wordt met een factor 10 geschaald zodat deze als een positie in cm opgelegd kunnen worden.
//Input = Filter2.step(Inp);
- Input = 6+(3*-cos(time_));
+ Input = 5.5+(2.5*-cos(2*time_));
if (Input>=9) { // De Input moet binnnen een aantal grenzen blijven. Groter dan 7 is mechanisch niet mogelijk.
Input=9; // Bij een waarde kleiner dan 1,5 zijn de strings niet meer gewikkeld en werkt de controller averechts en is deze uiterst instabiel.
} else if (Input<=3.0) {
@@ -87,13 +89,14 @@
double Errorberekening(double Input,double Ref)
{
Error = Input-Ref; // Het Error-signaal wordt ook gebruikt voor de PWMOut, dus mag deze niet hoger worden dan 1, 1 is immers al full speed voor de motor.
- if (Error>=1) {
+ /*if (Error>=1) {
Error=1;
} else if (Error<=-1) {
Error = -1;
} else if (fabs(Error)<0.01) {
Error = 0;
}
+ */
return Error;
}
double PID_controller(double Error, double KP, double KI, double KD, double Ts, double &Error_int, double &e_prev)
@@ -123,148 +126,156 @@
void Motor_controller() // De P_controller, door de ticker elke honderdste seconde uitgevoerd.
{
- if(Output>=0) {
- motor1direction.write(1);
- motor1speed = 0.3*fabs(Output);
- } else if (Output<0) {
- motor1direction.write(0);
- motor1speed = fabs(Output);
- }
-}
-
-void Extendfinger()
-{
- Error = 0;
- int J = 0;
- time_ = 0; // Voordat het hele programma begint, staat de Error op 0, zodat de motor niet spastisch gaat draaien om dit te compenseren.
- while(1 && J<=640) {
- Loopticker.attach(tickerfunctie2,0.01);
- Hidscope.attach(scopeSend,0.01); // Verzenden naar HIDscope
- while(1 && J<=640) {
- while(LoopTimerFlag2 !=1); // Als LTF 0 is, blijft hij 0 en stopt de loop.
- LoopTimerFlag2 = 0; // Als voorgaand statement niet waar is, maken we de LTF weer 0 en gaan we verder met het programma
- J= J+1;
- pc.printf(" J is %i \n",J);
- Led = 1;
- double Input = Inputberekening(Boardpotmeter.read());
- time_ += time_increment;
- double Ref2 = Referentieschaling(Referentie.read()/2,Input); // De referentiewaarde is via deze functie (gevonden met metingen en polyfit) verbonden aan de afstand in centimeters voor waarden tussen 0 en 0.5.
- Error = Errorberekening(Referentie2, Input);
- Output = PID_controller(Error,1,0.5,1,0.01, Error_prev, Error_int);
- Motor_controller();
+ if(Ref_der < 0) {
+ if(Output>=0) {
+ motor1direction.write(1);
+ motor1speed = 0.01*fabs(Output);
+ } else if (Output<0) {
+ motor1direction.write(0);
+ motor1speed = 0.05*fabs(Output);
}
}
- motor1speed.write(0);
-
-}
-
-void tickerfunctie()
-{
- LoopTimerFlag = 1;
-}
-
-void Determinetask()
-{
- if (Button1pressed == 0) {
-
- Groen = 0;
- N++;
- wait (0.5);
- Groen = 1;
- }
-}
-
-/* void Extendfinger()
-{
- //i = i+1;
- wait (0.5); // Knipper 2 keer met Rood en 1 keer met paars
- Rood = 0;
- wait (0.5);
- Rood = 1;
- wait (0.5);
- Rood = 0;
- wait (0.5);
- Rood = 1;
- wait (0.5);
- Rood = 0;
- Blauw = 0;
- wait (0.5);
- Rood = 1;
- Blauw = 1;
- wait (0.5);
-} */
-
-int main()
-{
- Rood = 1;
- Blauw = 1;
- Groen = 1;
- int i = 0;
- pc.printf("Hello World! %i \n", i);
- Finitestatemachine.attach(tickerfunctie,0.1);
-
- while (true) {
- while(LoopTimerFlag !=1); // Als LTF 0 is, blijft hij 0 en stopt de loop.
- LoopTimerFlag = 0; // Als voorgaand statement niet waar is, maken we de LTF weer 0 en gaan we verder met het programma
- if (Start==0 && Setting==0 && Dotask==0) { // In eerste instantie gaan de leds even uit
-
- Start = 1;
- Groen = 0;
- pc.printf("State 1\n");
- }
-
- if (Start==1 && Setting==0 && Dotask==0) { // State 2 heeft Geel als kleur
- Groen = 0;
- Rood = 0;
- pc.printf("State 2\n");
- if (Button1pressed.read()==1 && Button2pressed.read()==0) {
- Setting = true;
- Start = false;
- Groen = 1;
- Rood = 1;
- pc.printf("State 2 A\n");
- wait(0.5);
-
-
- }
- if (Button1pressed.read()==0 && Button2pressed.read()==1 && N>0) {
- Groen = 1;
- Rood = 1;
- Dotask = 1;
- Start=0;
- pc.printf("State 2 B\n");
- wait(0.5);
- }
- }
-
- if (Start==0 && Setting==1 && Dotask==0) { // Leds zijn uit, elke count gaat het groene lampje even branden.
- pc.printf("State 3\n");
- Determinetask(); // Aantal gewenste herhalingen van Dotask instellen
- pc.printf("N = %i \n", N);
- if (Button1pressed.read()== 1 && Button2pressed.read()== 0) {
- Setting = 0;
- Dotask = 1;
- pc.printf("State 3 B\n");
- }
- }
-
- if (Start==0 && Setting==0 && Dotask==1) {
- pc.printf("State 4\n");
- while(i < N) {
-
- pc.printf("i = %i N = %i \n", i, N);
- Extendfinger();
- //int J = 0;
- pc.printf("Extendfinger()afgerond\n", i, N);
- i++;
- }
- if (i==N) {
- Dotask = 0;
- Start = 1;
- i=0;
- pc.printf("Final state \n");
- }
-
+ if(Ref_der >= 0) {
+ if(Output>=0) {
+ motor1direction.write(1);
+ motor1speed = (20*fabs(Output));
+ } else if (Output<0) {
+ motor1direction.write(0);
+ motor1speed = 0.01*fabs(Output);
}
}
}
+ void Extendfinger(){
+ Error = 0;
+ int J = 0;
+ time_ = 0; // Voordat het hele programma begint, staat de Error op 0, zodat de motor niet spastisch gaat draaien om dit te compenseren.
+ while(1 && J<=320) {
+ Loopticker.attach(tickerfunctie2,0.01);
+ Hidscope.attach(scopeSend,0.01); // Verzenden naar HIDscope
+ while(1 && J<=320) {
+ while(LoopTimerFlag2 !=1); // Als LTF 0 is, blijft hij 0 en stopt de loop.
+ LoopTimerFlag2 = 0; // Als voorgaand statement niet waar is, maken we de LTF weer 0 en gaan we verder met het programma
+ J= J+1;
+ pc.printf(" J is %i \n",J);
+ Led = 1;
+ double Input = Inputberekening(Boardpotmeter.read());
+ time_ += time_increment;
+ double Ref2 = Referentieschaling(Referentie.read()/2,Input); // De referentiewaarde is via deze functie (gevonden met metingen en polyfit) verbonden aan de afstand in centimeters voor waarden tussen 0 en 0.5.
+ Ref_der = Referentie2-Ref_prev;
+ Ref_prev = Referentie2;
+ Error = Errorberekening(Input, Referentie2);
+ Output = PID_controller(Error,5,1,0.1,0.01, Error_prev, Error_int);
+ Motor_controller();
+ }
+ }
+ motor1speed.write(0);
+
+ }
+
+ void tickerfunctie() {
+ LoopTimerFlag = 1;
+ }
+
+ void Determinetask() {
+ if (Button1pressed == 0) {
+
+ Groen = 0;
+ N++;
+ wait (0.5);
+ Groen = 1;
+ }
+ }
+
+ /* void Extendfinger()
+ {
+ //i = i+1;
+ wait (0.5); // Knipper 2 keer met Rood en 1 keer met paars
+ Rood = 0;
+ wait (0.5);
+ Rood = 1;
+ wait (0.5);
+ Rood = 0;
+ wait (0.5);
+ Rood = 1;
+ wait (0.5);
+ Rood = 0;
+ Blauw = 0;
+ wait (0.5);
+ Rood = 1;
+ Blauw = 1;
+ wait (0.5);
+ } */
+
+ int main() {
+ Rood = 1;
+ Blauw = 1;
+ Groen = 1;
+ int i = 0;
+ pc.printf("Hello World! %i \n", i);
+ Finitestatemachine.attach(tickerfunctie,0.1);
+
+ while (true) {
+ while(LoopTimerFlag !=1); // Als LTF 0 is, blijft hij 0 en stopt de loop.
+ LoopTimerFlag = 0; // Als voorgaand statement niet waar is, maken we de LTF weer 0 en gaan we verder met het programma
+ if (Start==0 && Setting==0 && Dotask==0) { // In eerste instantie gaan de leds even uit
+
+ Start = 1;
+ Groen = 0;
+ pc.printf("State 1\n");
+ }
+
+ if (Start==1 && Setting==0 && Dotask==0) { // State 2 heeft Geel als kleur
+ Groen = 0;
+ Rood = 0;
+ pc.printf("State 2\n");
+ if (Button1pressed.read()==1 && Button2pressed.read()==0) {
+ Setting = true;
+ Start = false;
+ Groen = 1;
+ Rood = 1;
+ pc.printf("State 2 A\n");
+ wait(0.5);
+
+
+ }
+ if (Button1pressed.read()==0 && Button2pressed.read()==1 && N>0) {
+ Groen = 1;
+ Rood = 1;
+ Dotask = 1;
+ Start=0;
+ pc.printf("State 2 B\n");
+ wait(0.5);
+ }
+ }
+
+ if (Start==0 && Setting==1 && Dotask==0) { // Leds zijn uit, elke count gaat het groene lampje even branden.
+ pc.printf("State 3\n");
+ Determinetask(); // Aantal gewenste herhalingen van Dotask instellen
+ pc.printf("N = %i \n", N);
+ if (Button1pressed.read()== 1 && Button2pressed.read()== 0) {
+ Setting = 0;
+ Dotask = 1;
+ pc.printf("State 3 B\n");
+ }
+ }
+
+ if (Start==0 && Setting==0 && Dotask==1) {
+ pc.printf("State 4\n");
+ while(i < N) {
+
+ pc.printf("i = %i N = %i \n", i, N);
+ Extendfinger();
+ //int J = 0;
+ pc.printf("Extendfinger()afgerond\n", i, N);
+ i++;
+ }
+ if (i==N) {
+ Dotask = 0;
+ Start = 1;
+ i=0;
+ pc.printf("Final state \n");
+ }
+
+ }
+ }
+ }