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:
- 2:1dd9e630a7b5
- Parent:
- 1:f63d8a73460c
- Child:
- 3:9f9ef68a25a2
--- a/main.cpp Mon May 30 13:14:25 2016 +0000
+++ b/main.cpp Tue May 31 14:53:58 2016 +0000
@@ -16,7 +16,7 @@
int N;
bool Start(0);
-bool Setting(0);
+double Setting(0);
bool Dotask(0);
int i;
@@ -25,18 +25,22 @@
// Verklaren van de in en outputs
Ticker Loopticker; // Deze ticker wordt gebruikt om de looptimerflag aan te roepen.
volatile bool LoopTimerFlag2 = 0; // Volatile, omdat deze heel vaak verandert van waar naar onwaar.
+Ticker Loopticker2; // Deze ticker wordt gebruikt om de looptimerflag aan te roepen.
+volatile bool LoopTimerFlag3 = 0;
AnalogIn Referentie(A0); // De schuifpotmeter naast de geleiding van de draad is de referentie.
-AnalogIn Boardpotmeter(A1); // POT1 op het board wordt gebruikt als input van de controlloop.
+AnalogIn Boardpotmeter(A2); // POT1 op het board wordt gebruikt als input van de controlloop.
+AnalogIn Boardpotmeter2(A1);
DigitalOut motor1direction(D7); //D6 en D7 voor motor 1 (op het motorshield)
FastPWM motor1speed(D6);
DigitalOut Led(LED_RED);
-AnalogIn Safety(A5);
+
float potmeter;
float Error;
float Referentie2=1.0;
float Input;
float Output;
+float POT;
double Error_prev = 0;
double Error_int = 0;
double Error_der;
@@ -74,8 +78,6 @@
double Inputberekening(double B)
{
- //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 = 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.
@@ -85,6 +87,12 @@
return Input;
}
+/*double Inputberekening2(double B, double Val)
+{
+ 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.
+ Val = Filter2.step(Inp);
+ return Inp;
+}*/
double Errorberekening(double Input,double Ref)
{
@@ -124,6 +132,11 @@
LoopTimerFlag2 = 1;
}
+void tickerfunctie3() //Deze functie wordt elke honderdste seconde aangeroepen en zorgt ervoor dat de while-loop helemaal uitgevoerd kan worden.
+{
+ LoopTimerFlag3 = 1;
+}
+
void Motor_controller() // De P_controller, door de ticker elke honderdste seconde uitgevoerd.
{
if(Ref_der < 0) {
@@ -145,137 +158,188 @@
}
}
}
- 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.
+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) {
- 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();
- }
+ 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);
+ }
+ motor1speed.write(0);
- }
+}
- void tickerfunctie() {
- LoopTimerFlag = 1;
- }
+void tickerfunctie()
+{
+ LoopTimerFlag = 1;
+}
+
+/*void Determinemax()
+{
+ double Val = 10*Boardpotmeter.read(); // 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.
+ double Max = Filter2.step(Val);
+ double Set_Error = Referentie.read()-Max;
+}*/
- void Determinetask() {
- if (Button1pressed == 0) {
-
- Groen = 0;
- N++;
- wait (0.5);
- Groen = 1;
+double P_controller(double Max, bool Keuze)
+{
+ while (Button1pressed.read()==1) {
+ Loopticker2.attach(tickerfunctie3,0.01);
+ while(LoopTimerFlag3 !=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
+ //pc.printf("Hello World! \n");
+ if (bool (0)) {
+ POT = Boardpotmeter.read();
+ } else if (bool (1)) {
+ POT = Boardpotmeter2.read();
}
+ float Input = 10*POT; //Inputberekening2(POT,0); // 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.
+ //pc.printf(" J is %i \n",J);
+ Led = 1;
+ float 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 = Ref2-Ref_prev;
+ Ref_prev = Ref2;
+ Error = Input-Ref2;
+ pc.printf("Input = %f Referentie= %f Error= %f \n", Input, Ref2, Error);
+ Output = PID_controller(Error,5,1,0.1,0.01, Error_prev, Error_int);
+ Motor_controller();
+
}
- /* 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;
+
+
+ /* //double Max = Filter2.step(Val);
+ double Ref3 = Referentieschaling(Referentie.read()/2,Val);
+ double Set_Error = Ref3-Val;
+ pc.printf("Maximum = %d Referentie= %d Error= %d \n", Val, Ref3, Set_Error);
+ Output = Set_Error;
+ if(Set_Error>=0.02) {
+ motor1direction.write(1);
+ motor1speed = fabs(Set_Error);
+ }
+ else if (Set_Error<-0.02) {
+ motor1direction.write(0);
+ motor1speed = fabs(Set_Error);
+ }
+ else {
+ motor1speed = 0;
+ } */
+
+
+ Groen = 0;
+ wait (0.5);
+ return Max;
+}
+
+void Determinetask()
+{
+ if (Button1pressed == 0) {
+
+ Groen = 0;
+ N++;
wait (0.5);
- } */
+ Groen = 1;
+ }
+}
+
+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);
+
- int main() {
- Rood = 1;
- Blauw = 1;
- Groen = 1;
- int i = 0;
- pc.printf("Hello World! %i \n", i);
- Finitestatemachine.attach(tickerfunctie,0.1);
+ }
+ 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 2 C\n");
+ double Maximum = P_controller(0,0);
+ Groen = 1;
+ pc.printf("Max = %f \n", Maximum);
+ double Minimum = P_controller(0,1);
+ Groen = 1;
+ pc.printf("Min = %f \n", Minimum);
+ Setting = 2;
+ }
- 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
+ if (Start==0 && Setting==2 && 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;
- Groen = 0;
- pc.printf("State 1\n");
+ i=0;
+ pc.printf("Final state \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");
- }
-
- }
}
}
+}