Peter Bartels
Laatste versie van ons script
Dependencies: Encoder HIDScope MODSERIAL TextLCD mbed-dsp mbed
Fork of
Main-script_groep7_V3
by 
Peter Bartels
Diff: main.cpp
- Revision:
- 7:7e3e183bf063
- Parent:
- 6:a7379a681adf
- Child:
- 8:f733c6a27c15
--- a/main.cpp Fri Oct 31 12:36:14 2014 +0000
+++ b/main.cpp Fri Oct 31 17:14:30 2014 +0000
@@ -81,6 +81,8 @@
int delta_pos_motor1_puls;
int aantal_pieken;
int doel;
+int doel_richting;
+int doel_hoogte;
bool aanspan;
void clamp(float * in, float min, float max);
volatile bool looptimerflag;
@@ -114,7 +116,7 @@
//HIDScope scope(6);
-enum state_t {RUST, ARM_KALIBRATIE, EMG_KALIBRATIE, METEN_RICHTING, METEN_HOOGTE, INSTELLEN_RICHTING, SLAAN, RETURN2RUST}; //verschillende stadia definieren voor gebruik in CASES
+enum state_t {RUST, EMG_KALIBRATIE, ARM_KALIBRATIE, METEN_HOOGTE, METEN_RICHTING, INSTELLEN_RICHTING, SLAAN, RETURN2RUST}; //verschillende stadia definieren voor gebruik in CASES
state_t state = RUST;
//functies die vanuit de statemachinefunction aangeroepen kunnen worden
@@ -122,13 +124,6 @@
current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen;
}
-void arm_kalibratie() {
- //voor nu om de loop te doorlopen wordt onderstaande code gebruikt. Nogmaal gesproken moet er gewacht worden op een 'hoog' signaal van een knop
- current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen;
- motor1.setPosition(0);
- motor2.setPosition(0);
-}
-
void pieken_tellen(){
if (emg_filtered>=emg_treshhold_hoog)
{
@@ -174,21 +169,47 @@
}
void emg_kalibratie() {
- if(emg_filtered>=0.05){
+ //if(emg_filtered>=0.05){//Deze if-loop alleen zodat het nog op de hidscope kan worden gezien, dit mag weg wanneer er een display is, current_herhalingen wel laten.
current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen;
- }
+ //}
if(current_herhalingen<=1000)
{
emg_max_meting();
}
}
-void meten_richting() {
+void arm_kalibratie() {
+ //voor nu om de loop te doorlopen wordt onderstaande code gebruikt. Nogmaal gesproken moet er gewacht worden op een 'hoog' signaal van een knop
current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen;
+ motor1.setPosition(0);
+ motor2.setPosition(0);
+}
+
+void doel_bepaling() {
+ if(200<=current_herhalingen<=1200){
+ emg_filtering();
+ }
+ else if(1600<=current_herhalingen<=2200){
+ emg_filtering();
+ }
+ else{
+ }
}
void meten_hoogte() {
current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen;
+ doel_bepaling();
+ if(1600<=current_herhalingen<=2200 && aantal_pieken==1){
+ doel=doel_hoogte;
+ }
+}
+
+void meten_richting() {
+ current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen;
+ doel_bepaling();
+ if(1600<=current_herhalingen<=2200 && aantal_pieken==1){
+ doel=doel_richting;
+ }
}
void instellen_richting() {
@@ -258,8 +279,6 @@
//scope.send();
}
-
-
float pid(float setpoint, float measurement)
{
float error;
@@ -330,9 +349,33 @@
emg_kalibratie();
if (current_herhalingen >=1000)
{
+ state = METEN_HOOGTE;
+ current_herhalingen = 0;
+ previous_herhalingen = 0;
+ aantal_pieken = 0;
+ doel = 0;
+ }
+ break;
+ }
+
+ case METEN_HOOGTE:
+ {
+ meten_hoogte();
+ if (current_herhalingen == 2800 && aantal_pieken == 1)
+ {
state = METEN_RICHTING;
current_herhalingen = 0;
previous_herhalingen = 0;
+ aantal_pieken = 0;
+ doel = 0;
+ }
+ else
+ {
+ state = METEN_HOOGTE;
+ current_herhalingen = 0;
+ previous_herhalingen = 0;
+ aantal_pieken = 0;
+ doel = 0;
}
break;
}
@@ -340,23 +383,21 @@
case METEN_RICHTING:
{
meten_richting();
- if (current_herhalingen == 100)
- {
- state = METEN_HOOGTE;
- current_herhalingen = 0;
- previous_herhalingen = 0;
- }
- break;
- }
-
- case METEN_HOOGTE:
- {
- meten_hoogte();
- if (current_herhalingen == 100)
+ if (current_herhalingen == 2800 && aantal_pieken == 1)
{
state = INSTELLEN_RICHTING;
current_herhalingen = 0;
previous_herhalingen = 0;
+ aantal_pieken = 0;
+ doel = 0;
+ }
+ else
+ {
+ state = METEN_RICHTING;
+ current_herhalingen = 0;
+ previous_herhalingen = 0;
+ aantal_pieken = 0;
+ doel = 0;
}
break;
}
@@ -443,6 +484,104 @@
lcd.printf("nog 1 sec.");
}
}
+ else if(state==METEN_HOOGTE){
+ lcd.cls();
+ if(current_herhalingen<=200){
+ lcd.locate(0,0);
+ lcd.printf("Hoogte bepalen:");
+ lcd.locate(0,1);
+ lcd.printf("span aan per vak");
+ }
+ else if(200<=current_herhalingen<=1200){
+ lcd.locate(0,0);
+ lcd.printf("Vak %d",doel);
+ if(current_herhalingen<=400){
+ lcd.locate(0,1);
+ lcd.printf("nog 5 sec.");
+ }
+ else if(current_herhalingen<=600){
+ lcd.locate(0,1);
+ lcd.printf("nog 4 sec.");
+ }
+ else if(current_herhalingen<=800){
+ lcd.locate(0,1);
+ lcd.printf("nog 3 sec.");
+ }
+ else if(current_herhalingen<=1000){
+ lcd.locate(0,1);
+ lcd.printf("nog 2 sec.");
+ }
+ else if(current_herhalingen<=1200){
+ lcd.locate(0,1);
+ lcd.printf("nog 1 sec.");
+ }
+ }
+ else if(current_herhalingen<=1600){
+ lcd.locate(0,0);
+ lcd.printf("Vak %d akkoord?",doel);
+ lcd.locate(0,1);
+ lcd.printf("Span aan");
+ }
+ else if(1600<=current_herhalingen<=2800 && aantal_pieken==1){
+ lcd.locate(0,0);
+ lcd.printf("Vak %d akkoord",doel_hoogte);
+ }
+ else{
+ lcd.locate(0,0);
+ lcd.printf("Opnieuw hoogte");
+ lcd.locate(0,1);
+ lcd.printf("bepalen");
+ }
+ }
+ else if(state==METEN_RICHTING){
+ lcd.cls();
+ if(current_herhalingen<=200){
+ lcd.locate(0,0);
+ lcd.printf("Richting bepalen:");
+ lcd.locate(0,1);
+ lcd.printf("span aan per vak");
+ }
+ else if(200<=current_herhalingen<=1200){
+ lcd.locate(0,0);
+ lcd.printf("Vak %d",doel);
+ if(current_herhalingen<=400){
+ lcd.locate(0,1);
+ lcd.printf("nog 5 sec.");
+ }
+ else if(current_herhalingen<=600){
+ lcd.locate(0,1);
+ lcd.printf("nog 4 sec.");
+ }
+ else if(current_herhalingen<=800){
+ lcd.locate(0,1);
+ lcd.printf("nog 3 sec.");
+ }
+ else if(current_herhalingen<=1000){
+ lcd.locate(0,1);
+ lcd.printf("nog 2 sec.");
+ }
+ else if(current_herhalingen<=1200){
+ lcd.locate(0,1);
+ lcd.printf("nog 1 sec.");
+ }
+ }
+ else if(current_herhalingen<=1600){
+ lcd.locate(0,0);
+ lcd.printf("Vak %d akkoord?",doel);
+ lcd.locate(0,1);
+ lcd.printf("Span aan");
+ }
+ else if(1600<=current_herhalingen<=2800 && aantal_pieken==1){
+ lcd.locate(0,0);
+ lcd.printf("Vak %d akkoord",doel_richting);
+ }
+ else{
+ lcd.locate(0,0);
+ lcd.printf("Opnieuw richting");
+ lcd.locate(0,1);
+ lcd.printf("bepalen");
+ }
+ }
else{
lcd.cls();
lcd.printf("state %d", state); //hier nog aan toevoegen hoe je de 'waarde', dus eigenlijk tekst, die opgeslagen staat in state kan printen.