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:
- 9:7e9b63fe8988
- Parent:
- 8:f733c6a27c15
- Child:
- 10:cd89569cd847
--- a/main.cpp Sat Nov 01 13:20:22 2014 +0000
+++ b/main.cpp Sat Nov 01 16:29:46 2014 +0000
@@ -57,13 +57,19 @@
arm_biquad_casd_df1_inst_f32 notch; //2e orde lowpass biquad butterworthfilter 50Hz
int previous_herhalingen = 0;
int current_herhalingen = 0;
+int current_herhalingen_1 = 0;
+int previous_herhalingen_1 = 0;
int previous_pos_motor1 = 0;
int current_pos_motor1;
+int current_pos_motor2;
int EMG = 1;
int aantal_pieken;
int doel;
int doel_richting;
int doel_hoogte;
+int puls_richting1;
+int puls_richting2;
+int puls_richting3;
bool aanspan;
void clamp(float * in, float min, float max);
float pid(float setpoint, float measurement);
@@ -85,6 +91,10 @@
float emg_max = 0;
float emg_treshhold_laag = 0;
float emg_treshhold_hoog = 0;
+float marge = 0;
+float PWM_richting1;
+float PWM_richting2;
+float PWM_richting3;
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;
@@ -119,12 +129,12 @@
arm_biquad_cascade_df1_f32(&lowpass_2, &emg_filtered, &emg_filtered, 1 );
scope.set(0,emg_value); //uint value
scope.set(1,emg_filtered); //processed float
- scope.set(2,doel);
- scope.send();
if(state!=EMG_KALIBRATIE)
{
pieken_tellen();
}//if
+ scope.set(2,doel);
+ scope.send();
}//void emg_filtering()
void emg_max_meting(){
@@ -137,6 +147,10 @@
emg_treshhold_hoog = 0.7*emg_max;
}//void emg_max_meting
+void akkoord_geven(){
+ emg_filtering();
+}
+
void emg_kalibratie() {
//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;
@@ -149,17 +163,27 @@
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);
+ motor2.setPosition(0);
+ pwm_motor1.period_us(100);
+ pwm_motor2.period_us(100);
+ akkoord_geven();
}//void arm_kalibratie
void doel_bepaling() {
- if(200<=current_herhalingen<=1200){
+ if(200<=current_herhalingen<1200){
emg_filtering();
}//if
- else if(1600<=current_herhalingen<=2200){
- emg_filtering();
+ else if(current_herhalingen == 1200 && state==METEN_HOOGTE){
+ doel_hoogte = doel;
+ aantal_pieken = 0;
+ }
+ else if(current_herhalingen == 1200 && state==METEN_RICHTING){
+ doel_richting = doel;
+ aantal_pieken = 0;//op 0 omdat bij akkoord geven dit ook gebruikt wordt.
+ }
+ else if(1200<current_herhalingen<=2200){
+ akkoord_geven();
}//else if
else{
}//else
@@ -168,33 +192,76 @@
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;
- }//if
}//void meten_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;
- }//if
}//void meten_richting
void instellen_richting() {
- current_herhalingen = previous_herhalingen + 1; previous_herhalingen = current_herhalingen;
+ current_pos_motor2 = motor2.getPosition();
+if (doel_richting ==1){
+ if (state==RETURN2RUST){
+ motordir2 = 1;
+ while (current_pos_motor2 > 0){
+ pwm_motor2.write(PWM_richting1);
+ current_herhalingen_1 = previous_herhalingen_1 + 1; previous_herhalingen_1 = current_herhalingen_1;
+ }//while (current_pos_motor2 < rad_richting1)
+ }//if (doel_richting == 1)
+ else{
+ motordir2 = 0;
+ while (current_pos_motor2 < puls_richting1){
+ pwm_motor2.write(PWM_richting1);
+ current_herhalingen_1 = previous_herhalingen_1 + 1; previous_herhalingen_1 = current_herhalingen_1;
+ }//while (current_pos_motor2 < rad_richting1)
+ }//if (doel_richting == 1)
+}//if (doel_richting ==1)
+
+
+else if (doel_richting == 2){
+ if (state==RETURN2RUST){
+ motordir2 = 1;
+ while (current_pos_motor2 > 0){
+ pwm_motor2.write(PWM_richting2);
+ current_herhalingen_1 = previous_herhalingen_1 + 1; previous_herhalingen_1 = current_herhalingen_1;
+ }//while (current_pos_motor2 < rad_richting1)
+ }//if (doel_richting == 1)
+ else{
+ motordir2 = 0;
+ while (current_pos_motor2 < puls_richting2){
+ pwm_motor2.write(PWM_richting2);
+ current_herhalingen_1 = previous_herhalingen_1 + 1; previous_herhalingen_1 = current_herhalingen_1;
+ }//while (current_pos_motor2 < rad_richting1)
+ }//if (doel_richting == 1)
+}//if (doel_richting ==1)
+
+else if(doel_richting == 3){
+ if (state==RETURN2RUST){
+ motordir2 =1;
+ while (current_pos_motor2 > 0){
+ pwm_motor2.write(PWM_richting3);
+ current_herhalingen_1 = previous_herhalingen_1 + 1; previous_herhalingen_1 = current_herhalingen_1;
+ }//while (current_pos_motor2 < rad_richting1)
+ }//if (doel_richting == 1)
+ else{
+ motordir2 = 0;
+ while (current_pos_motor2 < puls_richting3){
+ pwm_motor2.write(PWM_richting3);
+ current_herhalingen_1 = previous_herhalingen_1 + 1; previous_herhalingen_1 = current_herhalingen_1;
+ }//while (current_pos_motor2 < rad_richting1)
+ }//if (doel_richting == 1)
+}//if (doel_richting ==1)
}//void instellen_richting
-void GotoPosition (float position_setpoint_rad, float speed_radpersecond) {
+void GotoPosition (float position_setpoint_rad, float speed_radpersecond, float marge){
current_pos_motor1 = motor1.getPosition(); //bekijk na elke 0.005s wat de huidige 'waarde' van de encoder is
pos_motor1_rad = current_pos_motor1/(1600/(2*PI)); //echte waarde omrekenen naar rad voor (positie) PID regelaar
-
previous_pos_motor1 = current_pos_motor1; //sla de huidige waarde op als vorige waarde.
- //nu gaan we positie regelen i.p.v. snelheid.
-
- if (fabs(pos_motor1_rad - position_setpoint_rad) <= 0.05) //if position error < ...rad, then stop.
+ //nu gaan we snelheid volgen d.m.v. positie regeling
+ if (fabs(pos_motor1_rad - position_setpoint_rad) <= marge) //if position error < ...rad, then stop.
{
speed_radpersecond = 0;
setpoint = pos_motor1_rad;
@@ -233,7 +300,7 @@
motordir1 = 0;
}//else
- pwm_motor1.write(abs(PWM1_percentage/100.))
+ pwm_motor1.write(abs(PWM1_percentage/100.));
prev_setpoint = setpoint;
}//void GotoPosition
@@ -268,60 +335,65 @@
{
current_herhalingen = 0;
previous_herhalingen = 0;
- state = ARM_KALIBRATIE;
- EMG = 0;
+ state = EMG_KALIBRATIE;
+ EMG = 0; //door EMG op 0 te zetten word deze loop nooit meer doorlopen, daarna zal altijd else worden uitgevoerd. Wat ook gelijk het kalibreren van de arm overslaat. Men kan na 1 keer kalibreren dus vaker achter elkaar schieten
}//if (current_herhalingen == 100 && EMG == 1)
+ else if(current_herhalingen == 100)
+ {
+ current_herhalingen = 0;
+ previous_herhalingen = 0;
+ state = METEN_HOOGTE;
+ }//else
break;
}//case RUST:
- case ARM_KALIBRATIE:
- {
- arm_kalibratie();
- if (current_herhalingen == 100)
- {
- current_herhalingen = 0;
- previous_herhalingen = 0;
- state = EMG_KALIBRATIE;
- motor1.setPosition(0);
- motor2.setPosition(0);
- pwm_motor1.period_us(100);
- pwm_motor2.period_us(100);
- }//if (current_herhalingen == 100)
- break;
- }//case ARM_KALIBRATIE:
-
case EMG_KALIBRATIE:
{
emg_kalibratie();
- if (current_herhalingen >=1000)
+ if (current_herhalingen >=1000) /*waarom >= en niet ==?*/
{
- state = METEN_HOOGTE;
current_herhalingen = 0;
previous_herhalingen = 0;
aantal_pieken = 0;
doel = 0;
+ state = ARM_KALIBRATIE;
}//if (current_herhalingen >=1000)
break;
}//case EMG_KALIBRATIE
+
+ case ARM_KALIBRATIE:
+ {
+ arm_kalibratie();
+ if (aantal_pieken == 1)
+ {
+ current_herhalingen = 0;
+ previous_herhalingen = 0;
+ aantal_pieken = 0;
+ doel = 0;
+ state = METEN_HOOGTE;
+ }//if (current_herhalingen == 100)
+ break;
+ }//case ARM_KALIBRATIE:
case METEN_HOOGTE:
{
meten_hoogte();
- if (current_herhalingen == 2800 && aantal_pieken == 1)
+ if (1200 < current_herhalingen <2200 && aantal_pieken == 1)
{
- state = METEN_RICHTING;
current_herhalingen = 0;
previous_herhalingen = 0;
aantal_pieken = 0;
doel = 0;
+ doel_hoogte = 0;
+ state = METEN_RICHTING;
}//if (current_herhalingen == 2800 && aantal_pieken == 1)
- else
+ else if (current_herhalingen == 2200)
{
- state = METEN_HOOGTE;
current_herhalingen = 0;
previous_herhalingen = 0;
aantal_pieken = 0;
- doel = 0;
+ doel = 0;
+ state = METEN_HOOGTE;
}///else
break;
}//case METEN_HOOGTE
@@ -329,22 +401,22 @@
case METEN_RICHTING:
{
meten_richting();
- if (current_herhalingen == 2800 && aantal_pieken == 1)
+ if (1200 < current_herhalingen <2200 && aantal_pieken == 1)
{
- state = INSTELLEN_RICHTING;
current_herhalingen = 0;
previous_herhalingen = 0;
aantal_pieken = 0;
doel = 0;
+ state = INSTELLEN_RICHTING;
}//if (current_herhalingen == 2800 && aantal_pieken == 1)
- else
+ else if (current_herhalingen == 2200)
{
- state = METEN_RICHTING;
current_herhalingen = 0;
previous_herhalingen = 0;
aantal_pieken = 0;
- doel = 0;
- }//else
+ doel = 0;
+ state = METEN_RICHTING;
+ }///else
break;
}//case METEN_RICHTING
@@ -353,9 +425,10 @@
instellen_richting();
if (current_herhalingen == 100)
{
+ current_herhalingen_1 = 0;
+ previous_herhalingen_1 = 0;
+ doel_richting = 0;
state = SLAAN;
- current_herhalingen = 0;
- previous_herhalingen = 0;
}//if (current_herhalingen == 100)
break;
}//case INSTELLEN_RICHTING
@@ -363,26 +436,31 @@
case SLAAN:
{
GotoPosition(1.5 ,8, 0.1);
- if (current_herhalingen == 100)
+ if (current_herhalingen == 400)
{
- state = RETURN2RUST;
current_herhalingen = 0;
previous_herhalingen = 0;
prev_setpoint =0;
setpoint =0;
+ state = RETURN2RUST;
}//if (current_herhalingen == 100)
break;
}//case SLAAN
case RETURN2RUST:
{
+ instellen_richting();
GotoPosition(0,4, 0.05);
- //if (current_herhalingen == 100)
- //{
- // state = RUST;
- // current_herhalingen = 0;
- // previous_herhalingen = 0;
- //}//if (current_herhalingen == 100)
+ doel_richting = 0;
+ doel_hoogte = 0;
+ if (current_herhalingen >= 200 && current_herhalingen_1 >= 200)
+ {
+ state = RUST;
+ current_herhalingen = 0;
+ previous_herhalingen = 0;
+ current_herhalingen = 0;
+ current_herhalingen = 0;
+ }//if (current_herhalingen == 100)
break;
}// case RETURN2RUST
@@ -429,6 +507,14 @@
}//else if(current_herhalingen<=1000)
}//else if(state==EMG_KALIBRATIE)
+ else if(state==ARM_KALIBRATIE){
+ lcd.cls();
+ lcd.locate(0,0);
+ lcd.printf("Set arm to zero");
+ lcd.locate(0,1);
+ lcd.printf("Klaar? Span aan");
+ }//else if(state==ARM_KALIBRATIE)
+
else if(state==METEN_HOOGTE){
lcd.cls();
if(current_herhalingen<=200){
@@ -437,7 +523,7 @@
lcd.locate(0,1);
lcd.printf("span aan per vak");
}//if(current_herhalingen<=200){
- else if(200<=current_herhalingen<=1200){
+ else if(200<=current_herhalingen<1200){
lcd.locate(0,0);
lcd.printf("Vak %d",doel);
if(current_herhalingen<=400){
@@ -456,21 +542,17 @@
lcd.locate(0,1);
lcd.printf("nog 2 sec.");
}//else if(current_herhalingen<=1000)
- else if(current_herhalingen<=1200){
+ else if(current_herhalingen<1200){
lcd.locate(0,1);
lcd.printf("nog 1 sec.");
}//else if(current_herhalingen<=1200)
}//else if(200<=current_herhalingen<=1200)
- else if(current_herhalingen<=1600){
+ else if(current_herhalingen<=2200){
lcd.locate(0,0);
- lcd.printf("Vak %d akkoord?",doel);
+ lcd.printf("Vak %d akkoord?",doel_hoogte);
lcd.locate(0,1);
lcd.printf("Span aan");
}//else if(current_herhalingen<=1600){
- else if(1600<=current_herhalingen<=2800 && aantal_pieken==1){
- lcd.locate(0,0);
- lcd.printf("Vak %d akkoord",doel_hoogte);
- }//else if(1600<=current_herhalingen<=2800 && aantal_pieken==1){
else{
lcd.locate(0,0);
lcd.printf("Opnieuw hoogte");
@@ -487,7 +569,7 @@
lcd.locate(0,1);
lcd.printf("span aan per vak");
}//if(current_herhalingen<=200)
- else if(200<=current_herhalingen<=1200){
+ else if(200<=current_herhalingen<1200){
lcd.locate(0,0);
lcd.printf("Vak %d",doel);
if(current_herhalingen<=400){
@@ -506,21 +588,17 @@
lcd.locate(0,1);
lcd.printf("nog 2 sec.");
}//else if(current_herhalingen<=1000)
- else if(current_herhalingen<=1200){
+ else if(current_herhalingen<1200){
lcd.locate(0,1);
lcd.printf("nog 1 sec.");
}//else if(current_herhalingen<=1200)
}//else if(200<=current_herhalingen<=1200)
- else if(current_herhalingen<=1600){
+ else if(current_herhalingen<=2200){
lcd.locate(0,0);
- lcd.printf("Vak %d akkoord?",doel);
+ lcd.printf("Vak %d akkoord?",doel_richting);
lcd.locate(0,1);
lcd.printf("Span aan");
}//else if(current_herhalingen<=1600)
- else if(1600<=current_herhalingen<=2800 && aantal_pieken==1){
- lcd.locate(0,0);
- lcd.printf("Vak %d akkoord",doel_richting);
- }//else if(1600<=current_herhalingen<=2800 && aantal_pieken==1)
else{
lcd.locate(0,0);
lcd.printf("Opnieuw richting");