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:
- 22:838a17065bc7
- Parent:
- 21:f4e9f6c28de1
- Child:
- 23:5267c928ae2b
--- a/main.cpp Fri Oct 31 12:37:06 2014 +0000
+++ b/main.cpp Fri Oct 31 14:08:57 2014 +0000
@@ -60,6 +60,7 @@
Timer triceps_kalibratie; //Timer voor kalibratiemeting EMG triceps
Ticker ticker_motor_arm1_pid;
Ticker ticker_motor_arm2_pid;
+Timer EMG;
//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
@@ -201,6 +202,15 @@
}
+void EMG_meten()
+{
+ xb = EMG_bi.read(); //EMG signaal biceps uitlezen
+ filter_biceps(); //EMG signaal biceps filteren
+ xt = EMG_tri.read(); //EMG signaal triceps uitlezen
+ filter_triceps(); //EMG signaal triceps filteren
+}
+
+
int main() //Main script
{
ticker_motor_arm1_pid.attach(motor_arm1_pid,SAMPLETIME_REGELAAR);
@@ -435,19 +445,22 @@
lcd.printf(" B "); //Tekst op LCD scherm
pc.printf("B\n\r"); //Controle naar pc sturen
- wait(3);
-
+ EMG.start();
+
+ while(EMG.read() <= 3)
+ {
+ while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
+ regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
+
+ EMG_meten();
+ }
+
while(state == B) {
while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
- xb = 0;
- xbf = 0;
- xb = EMG_bi.read(); //EMG signaal biceps uitlezen
- filter_biceps(); //EMG signaal biceps filteren
- xt = EMG_tri.read(); //EMG signaal triceps uitlezen
- filter_triceps(); //EMG signaal triceps filteren
-
+ EMG_meten();
+
if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
state = BB; //Ga door naar state BB
}
@@ -463,16 +476,21 @@
lcd.printf(" T "); //Tekst op LCD scherm
pc.printf("T\n\r"); //Controle naar pc sturen
- wait(3);
+ EMG.start();
+
+ while(EMG.read() <= 3)
+ {
+ while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
+ regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
+
+ EMG_meten();
+ }
while(state == T) {
while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
- xb = EMG_bi.read(); //EMG signaal biceps uitlezen
- filter_biceps(); //EMG signaal biceps filteren
- xt = EMG_tri.read(); //EMG signaal triceps uitlezen
- filter_triceps(); //EMG signaal triceps filteren
+ EMG_meten();
if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
state = TB; //Ga door naar state TB
@@ -489,17 +507,22 @@
lcd.printf(" BB "); //Tekst op LCD scherm
pc.printf("BB\n\r"); //Controle naar pc sturen
- wait(3);
+ EMG.reset();
+ EMG.start();
+
+ while(EMG.read() <= 3)
+ {
+ while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
+ regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
+
+ EMG_meten();
+ }
while(state == BB) {
while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
- xb = EMG_bi.read(); //EMG signaal biceps uitlezen
- xbf = 0;
- filter_biceps(); //EMG signaal biceps filteren
- xt = EMG_tri.read(); //EMG signaal triceps uitlezen
- filter_triceps(); //EMG signaal triceps filteren
+ EMG_meten();
if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
state = BBB; //Ga door naar state BBB
@@ -516,16 +539,22 @@
lcd.printf(" BT "); //Tekst op LCD scherm
pc.printf("BT\n\r"); //Controle naar pc sturen
- wait(3);
+ EMG.reset();
+ EMG.start();
+
+ while(EMG.read() <= 3)
+ {
+ while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
+ regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
+
+ EMG_meten();
+ }
while(state == BT) {
while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
- xb = EMG_bi.read(); //EMG signaal biceps uitlezen
- filter_biceps(); //EMG signaal biceps filteren
- xt = EMG_tri.read(); //EMG signaal triceps uitlezen
- filter_triceps(); //EMG signaal triceps filteren
+ EMG_meten();
if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
state = BTB; //Ga door naar state BTB
@@ -542,16 +571,22 @@
lcd.printf(" TB "); //Tekst op LCD scherm
pc.printf("TB\n\r"); //Controle naar pc sturen
- wait(3);
+ EMG.reset();
+ EMG.start();
+
+ while(EMG.read() <= 3)
+ {
+ while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
+ regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
+
+ EMG_meten();
+ }
while(state == TB) {
while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
- xb = EMG_bi.read(); //EMG signaal biceps uitlezen
- filter_biceps(); //EMG signaal biceps filteren
- xt = EMG_tri.read(); //EMG signaal triceps uitlezen
- filter_triceps(); //EMG signaal triceps filteren
+ EMG_meten();
if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
state = TBB; //Ga door naar state TBB
@@ -568,16 +603,22 @@
lcd.printf(" TT "); //Tekst op LCD scherm
pc.printf("TT\n\r"); //Controle naar pc sturen
- wait(3);
+ EMG.reset();
+ EMG.start();
+
+ while(EMG.read() <= 3)
+ {
+ while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
+ regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
+
+ EMG_meten();
+ }
while(state == TT) {
while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
- xb = EMG_bi.read(); //EMG signaal biceps uitlezen
- filter_biceps(); //EMG signaal biceps filteren
- xt = EMG_tri.read(); //EMG signaal triceps uitlezen
- filter_triceps(); //EMG signaal triceps filteren
+ EMG_meten();
if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
state = TTB; //Ga door naar state TTB
@@ -594,16 +635,22 @@
lcd.printf(" BBB "); //Tekst op LCD scherm
pc.printf("BBB\n\r"); //Controle naar pc sturen
- wait(3);
+ EMG.reset();
+ EMG.start();
+
+ while(EMG.read() <= 3)
+ {
+ while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
+ regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
+
+ EMG_meten();
+ }
while(state == BBB) {
while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
- xb = EMG_bi.read(); //EMG signaal biceps uitlezen
- filter_biceps(); //EMG signaal biceps filteren
- xt = EMG_tri.read(); //EMG signaal triceps uitlezen
- filter_triceps(); //EMG signaal triceps filteren
+ EMG_meten();
if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
state = BBBB; //Ga door naar state BBBB
@@ -621,16 +668,22 @@
lcd.printf(" BBT "); //Tekst op LCD scherm
pc.printf("BBT\n\r"); //Controle naar pc sturen
- wait(3);
+ EMG.reset();
+ EMG.start();
+
+ while(EMG.read() <= 3)
+ {
+ while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
+ regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
+
+ EMG_meten();
+ }
while(state == BBT) {
while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
- xb = EMG_bi.read(); //EMG signaal biceps uitlezen
- filter_biceps(); //EMG signaal biceps filteren
- xt = EMG_tri.read(); //EMG signaal triceps uitlezen
- filter_triceps(); //EMG signaal triceps filteren
+ EMG_meten();
if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
state = BBTB; //Ga door naar state BBTB
@@ -647,16 +700,22 @@
lcd.printf(" BTB "); //Tekst op LCD scherm
pc.printf("BTB\n\r"); //Controle naar pc sturen
- wait(3);
+ EMG.reset();
+ EMG.start();
+
+ while(EMG.read() <= 3)
+ {
+ while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
+ regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
+
+ EMG_meten();
+ }
while(state == BTB) {
while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
- xb = EMG_bi.read(); //EMG signaal biceps uitlezen
- filter_biceps(); //EMG signaal biceps filteren
- xt = EMG_tri.read(); //EMG signaal triceps uitlezen
- filter_triceps(); //EMG signaal triceps filteren
+ EMG_meten();
if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
state = BTBB; //Ga door naar state BTBB
@@ -673,16 +732,22 @@
lcd.printf(" BTT "); //Tekst op LCD scherm
pc.printf("BTT\n\r"); //Controle naar pc sturen
- wait(3);
+ EMG.reset();
+ EMG.start();
+
+ while(EMG.read() <= 3)
+ {
+ while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
+ regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
+
+ EMG_meten();
+ }
while(state == BTT) {
while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
- xb = EMG_bi.read(); //EMG signaal biceps uitlezen
- filter_biceps(); //EMG signaal biceps filteren
- xt = EMG_tri.read(); //EMG signaal triceps uitlezen
- filter_triceps(); //EMG signaal triceps filteren
+ EMG_meten();
if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
state = BTTB; //Ga door naar state BTTB
@@ -699,16 +764,22 @@
lcd.printf(" TBB "); //Tekst op LCD scherm
pc.printf("TBB\n\r"); //Controle naar pc sturen
- wait(3);
+ EMG.reset();
+ EMG.start();
+
+ while(EMG.read() <= 3)
+ {
+ while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
+ regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
+
+ EMG_meten();
+ }
while(state == TBB) {
while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
- xb = EMG_bi.read(); //EMG signaal biceps uitlezen
- filter_biceps(); //EMG signaal biceps filteren
- xt = EMG_tri.read(); //EMG signaal triceps uitlezen
- filter_triceps(); //EMG signaal triceps filteren
+ EMG_meten();
if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
state = TBBB; //Ga door naar state TBBB
@@ -725,16 +796,22 @@
lcd.printf(" TBT "); //Tekst op LCD scherm
pc.printf("TBT\n\r"); //Controle naar pc sturen
- wait(3);
+ EMG.reset();
+ EMG.start();
+
+ while(EMG.read() <= 3)
+ {
+ while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
+ regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
+
+ EMG_meten();
+ }
while(state == TBT) {
while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
- xb = EMG_bi.read(); //EMG signaal biceps uitlezen
- filter_biceps(); //EMG signaal biceps filteren
- xt = EMG_tri.read(); //EMG signaal triceps uitlezen
- filter_triceps(); //EMG signaal triceps filteren
+ EMG_meten();
if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
state = TBTB; //Ga door naar state TBTB
@@ -748,19 +825,25 @@
case TTB: {
lcd.locate(0,0); //Zet tekst op eerste regel
- lcd.printf(" BBB "); //Tekst op LCD scherm
- pc.printf("BBB\n\r"); //Controle naar pc sturen
+ lcd.printf(" TTB "); //Tekst op LCD scherm
+ pc.printf("TTB\n\r"); //Controle naar pc sturen
+
+ EMG.reset();
+ EMG.start();
- wait(3);
+ while(EMG.read() <= 3)
+ {
+ while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
+ regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
+
+ EMG_meten();
+ }
- while(state == BBB) {
+ while(state == TTB) {
while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
- xb = EMG_bi.read(); //EMG signaal biceps uitlezen
- filter_biceps(); //EMG signaal biceps filteren
- xt = EMG_tri.read(); //EMG signaal triceps uitlezen
- filter_triceps(); //EMG signaal triceps filteren
+ EMG_meten();
if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
state = TTBB; //Ga door naar state TTBB
@@ -777,16 +860,22 @@
lcd.printf(" TTT "); //Tekst op LCD scherm
pc.printf("TTT\n\r"); //Controle naar pc sturen
- wait(3);
+ EMG.reset();
+ EMG.start();
+
+ while(EMG.read() <= 3)
+ {
+ while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
+ regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
+
+ EMG_meten();
+ }
while(state == TTT) {
while(regelaar_EMG_flag != true); //Als geen flag opgestoken, dan niks doen, wel flag, dan uit de while loop en door naar volgende regel
regelaar_EMG_flag = false; //Flag weer naar beneden, zodat deze straks weer omhoog kan
- xb = EMG_bi.read(); //EMG signaal biceps uitlezen
- filter_biceps(); //EMG signaal biceps filteren
- xt = EMG_tri.read(); //EMG signaal triceps uitlezen
- filter_triceps(); //EMG signaal triceps filteren
+ EMG_meten();
if(xbf >= xbt) { //Als de gefilterde EMG van de biceps groter is dan de ingestelde threshold
state = TTTB; //Ga door naar state TTTB
@@ -910,10 +999,10 @@
case TBTB: {
lcd.locate(0,0); //Zet tekst op eerste regel
- lcd.printf(" TBBB "); //Tekst op LCD scherm
- pc.printf("TBBB\n\r"); //Controle naar pc sturen
+ lcd.printf(" TBTB "); //Tekst op LCD scherm
+ pc.printf("TBTB\n\r"); //Controle naar pc sturen
- while(state == TBBB) {
+ while(state == TBTB) {
//Motoractie
}
break; //Stop met alle acties in deze case