Aukie Hooglugt
richting is nog niet ingesteld, kan aan setpoint liggen
Dependencies: Encoder HIDScope MODSERIAL- mbed-dsp mbed
Diff: PROJECT_main.cpp
- Revision:
- 2:10dd6a88dfd7
- Parent:
- 1:d44a866de64f
- Child:
- 3:2b2710b8b02e
--- a/PROJECT_main.cpp Fri Oct 31 13:22:09 2014 +0000
+++ b/PROJECT_main.cpp Fri Oct 31 16:17:18 2014 +0000
@@ -10,6 +10,7 @@
#define TSAMP_EMG 0.002 //sample frequency emg
#define KALIBRATIONTIME 1000 // 10 sec voor bepalen van maximale biceps/triceps waarde
#define FACTOR 0.6 //factor*max_waarde = threshold emg
+
//Define objects
AnalogIn emg0(PTB1); //Analog input biceps
AnalogIn emg1(PTB2); //Analog input triceps
@@ -91,8 +92,8 @@
PwmOut pwm_motor2(PTA5);
float integral = 0;
-float batjeset = 0; //een if statement wordt "true" wanneer batje voor een bepaalde tijd de juiste hoek heeft behouden
-float balhit = 0; //balhit wordt 1 wanneer arm een bepaalde hoek heeft afgelegd
+uint8_t batjeset = 0; //een if statement wordt "true" wanneer batje voor een bepaalde tijd de juiste hoek heeft behouden
+uint8_t balhit = 0; //balhit wordt 1 wanneer arm een bepaalde hoek heeft afgelegd
float controlerror = 0;
float pwm = 0;
@@ -276,33 +277,28 @@
pwm_motor2.write(0.6); //lage PWM
motor2dir = 1;
wait(1); // anders wordt de while(1) meteen onderbroken
- while(1) {
- if(motor2.getSpeed()*omrekenfactor2 > -0.70 && motor2.getSpeed()*omrekenfactor2 < 0.70) { // ik weet niet of het rechtsom of linksom zal gaan en misschien is de speed bij de gekozen pwm wel binnen dit bereik
+ while(motor2.getSpeed()*omrekenfactor2>0) {
+ if(motor2.getSpeed()*omrekenfactor2 < 0.70) { //0.70 is nog aan te passen
pwm_motor2.write(0);
- motor2.setPosition(0);
- goto motor1cal;
+ motor2.setPosition(0);
}
wait(0.01);
}
-motor1cal:
//calibration motor 1
pwm_motor1.write(0.55); //lage PWM
- motor1dir = 1;
+ motor1dir = 0;
wait(1); // anders wordt de while(1) meteen onderbroken
- while(1) {
- if(motor1.getSpeed()*omrekenfactor1 > -0.20 && motor1.getSpeed()*omrekenfactor1 < 0.20) { // ik weet niet of het rechtsom of linksom zal gaan en misschien is de speed bij de gekozen pwm wel binnen dit bereik
+ while(motor1.getSpeed()*omrekenfactor1<0) {
+ if(motor1.getSpeed()*omrekenfactor1 > -0.20 ) { // 0.20 is nog aan te passen
pwm_motor1.write(0);
- motor1.setPosition(0);
- goto emgcal;
+ motor1.setPosition(0);
}
wait(0.01);
}
-emgcal:
blink.detach();
blink2.detach();
dir1 = dir2 = dir3 = 1;
- for1 = for2 = for3 = 1;
- pc.printf("kalmoarm ");
+ for1 = for2 = for3 = 1;
wait (1);
for1 = for2 = for3 = 0;
@@ -316,7 +312,6 @@
}
blink.detach();
dir1 = dir2 = dir3 = 1;
- pc.printf("kalbi ");
wait (1);
//triceps kalibratie
@@ -329,7 +324,6 @@
}
blink.detach();
for1 = for2 = for3 = 1;
- pc.printf("kaltri ");
wait (1);
for1 = for2 = for3 = 0;
@@ -345,7 +339,6 @@
for (int lag=0; lag<TIMEBETWEENBLINK; lag++) {
if(bi_result>FACTOR*bi_max) {
direction = 1;
- pc.printf("links ");
wait(TIMEB4NEXTCHOICE); // Tijdelijke wait om cyaan lampje aan te zetten ter controle selectie
goto forcechoice; // goes to second while(1) for the deciding the force
} else {
@@ -360,7 +353,6 @@
for (int lag=0; lag<TIMEBETWEENBLINK; lag++) {
if(bi_result>FACTOR*bi_max) {
direction = 2;
- pc.printf("mid ");
wait(TIMEB4NEXTCHOICE); // Tijdelijke wait om paars lampje aan te zetten ter controle selectie
goto forcechoice;
} else {
@@ -375,7 +367,6 @@
for (int lag=0; lag<TIMEBETWEENBLINK; lag++) {
if(bi_result>FACTOR*bi_max) {
direction = 3;
- pc.printf("rechts ");
wait(TIMEB4NEXTCHOICE); // Tijdelijke wait om oranje lampje aan te zetten ter controle selectie
goto forcechoice;
} else {
@@ -395,12 +386,10 @@
for (int lag=0; lag<TIMEBETWEENBLINK; lag++) {
if(tri_result>FACTOR*tri_max) {
for1 = for2 = for3 = 0;
- pc.printf("reset ");
goto directionchoice;
} else {
if(bi_result>FACTOR*bi_max) {
force = 1;
- pc.printf("zwak ");
wait(TIMEB4NEXTCHOICE); // Tijdelijke wait om cyaan lampje aan te zetten ter controle selectie
goto choicesmade;
} else {
@@ -416,12 +405,10 @@
for (int lag=0; lag<TIMEBETWEENBLINK; lag++) {
if(tri_result>FACTOR*tri_max) {
for1 = for2 = for3 = 0;
- pc.printf("reset ");
goto directionchoice;
} else {
if(bi_result>FACTOR*bi_max) {
force = 2;
- pc.printf("normaal ");
wait(TIMEB4NEXTCHOICE); // Tijdelijke wait om paars lampje aan te zetten ter controle selectie
goto choicesmade;
} else {
@@ -437,12 +424,10 @@
for (int lag=0; lag<TIMEBETWEENBLINK; lag++) {
if(tri_result>FACTOR*tri_max) {
for1 = for2 = for3 = 0;
- pc.printf("reset ");
goto directionchoice;
} else {
if(bi_result>FACTOR*bi_max) {
force = 3;
- pc.printf("sterk ");
wait(TIMEB4NEXTCHOICE); // Tijdelijke wait om oranje lampje aan te zetten ter controle selectie
goto choicesmade;
} else {
@@ -459,7 +444,6 @@
while(1) {
if(tri_result>FACTOR*tri_max) {
blink.detach();
- pc.printf("reset ");
switch (direction) {
case 1:
dir1 = 1;
@@ -497,22 +481,22 @@
switch (direction) {
case 1:
- setpoint2 = 3.14;
+ setpoint2 = -2*0.197222205;
break;
case 2:
- setpoint2 = 3.14;
+ setpoint2 = -1*0.197222205;
break;
case 3:
- setpoint2 = 3.14;
+ setpoint2 = 0;
break;
}
switch (force) {
case 1:
- setpoint1 = 8;
+ setpoint1 = 6.8;
break;
case 2:
- setpoint1 = 8;
+ setpoint1 = 7.4;
break;
case 3:
setpoint1 = 8;
@@ -647,8 +631,8 @@
} else {
pwm_motor2.write(0);
batjeset = integral = 0;
- wait(1);
direction = force = 0;
+ wait(1);
goto directionchoice;
}
}