Aukie Hooglugt
richting is nog niet ingesteld, kan aan setpoint liggen
Dependencies: Encoder HIDScope MODSERIAL- mbed-dsp mbed
Diff: PROJECT_main.cpp
- Revision:
- 1:d44a866de64f
- Parent:
- 0:99cbc87af37c
- Child:
- 2:10dd6a88dfd7
--- a/PROJECT_main.cpp Fri Oct 31 10:31:02 2014 +0000
+++ b/PROJECT_main.cpp Fri Oct 31 13:22:09 2014 +0000
@@ -6,16 +6,17 @@
#define TSAMP 0.001 // sample freq encoder motor
#define TIMEB4NEXTCHOICE 1 // sec keuzelampje blijft aan
-#define TIMEBETWEENBLINK 200 // sec voor volgende blink
+#define TIMEBETWEENBLINK 100 // sec voor volgende blink
#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
Ticker log_timer; //sample emg
-Ticker blink; //ledjes aan/uit
+Ticker blink; //ledjes aan/uit
+Ticker blink2; //extra tikker zodat kalbi en kaltri tegelijkertijd aankunnen
Ticker looptimer; //motor regelaar
MODSERIAL pc(USBTX,USBRX);
@@ -96,13 +97,10 @@
float pwm = 0;
float omrekenfactor1 = 0.0028035714; // 6.28/(32*70)
-float omrekenfactor2 = 0.0015213178; // 6.28/(24*172);
+float omrekenfactor2 = 0.0015213178; // 6.28/(24*172);
-float setpoint1 = 8; // te behalen speed van motor1 (37D)
-float setpoint2 = 3.14; // te behalen hoek van motor2 (25D)
-
-//float setpoint1 = 0; eigenlijk moeten deze waarden later in de if-statement bij motorcontrol bepaald worden
-//float setpoint2 = 0;
+float setpoint1 = 0; //te behalen speed van motor1 (37D)
+float setpoint2 = 0; //te behalen hoek van motor2 (25D)
float Kp1 = 1.10; //DEZE KP1 EN KP1 ZIJN NOG NIET DEFINITIEF
float Ki1 = 0.20; //Kp en Ki van motor1, voor de slag
@@ -272,7 +270,7 @@
//motorarm naar nul-positie
blink.attach(kalbi, 0.2);
- blink.attach(kaltri, 0.2);
+ blink2.attach(kaltri, 0.2);
//calibration motor 2
pwm_motor2.write(0.6); //lage PWM
@@ -301,6 +299,7 @@
}
emgcal:
blink.detach();
+ blink2.detach();
dir1 = dir2 = dir3 = 1;
for1 = for2 = for3 = 1;
pc.printf("kalmoarm ");
@@ -314,29 +313,28 @@
bi_max = bi_result;
}
wait (0.01);
-
- blink.detach();
- dir1 = dir2 = dir3 = 1;
- pc.printf("kalbi ");
- wait (1);
+ }
+ blink.detach();
+ dir1 = dir2 = dir3 = 1;
+ pc.printf("kalbi ");
+ wait (1);
- //triceps kalibratie
- blink.attach(kaltri, 0.2);
- for (int kaltime = 0; kaltime<KALIBRATIONTIME; kaltime++) {
- if (tri_max < tri_result) {
- tri_max = tri_result;
- }
- wait (0.01);
+ //triceps kalibratie
+ blink.attach(kaltri, 0.2);
+ for (int kaltime = 0; kaltime<KALIBRATIONTIME; kaltime++) {
+ if (tri_max < tri_result) {
+ tri_max = tri_result;
}
- blink.detach();
- for1 = for2 = for3 = 1;
- pc.printf("kaltri ");
- wait (1);
- for1 = for2 = for3 = 0;
+ wait (0.01);
}
+ blink.detach();
+ for1 = for2 = for3 = 1;
+ pc.printf("kaltri ");
+ wait (1);
+ for1 = for2 = for3 = 0;
directionchoice:
-log_timer.attach(looper, TSAMP_EMG);
+ log_timer.attach(looper, TSAMP_EMG);
while(1) { //Loop keuze DIRECTION
for(int i=1; i<4; i++) {
@@ -345,7 +343,7 @@
dir2=0;
dir3=0;
for (int lag=0; lag<TIMEBETWEENBLINK; lag++) {
- if(bi_result>0.8*bi_max) {
+ if(bi_result>FACTOR*bi_max) {
direction = 1;
pc.printf("links ");
wait(TIMEB4NEXTCHOICE); // Tijdelijke wait om cyaan lampje aan te zetten ter controle selectie
@@ -360,7 +358,7 @@
dir2 =1;
dir3 =0;
for (int lag=0; lag<TIMEBETWEENBLINK; lag++) {
- if(bi_result>0.8*bi_max) {
+ if(bi_result>FACTOR*bi_max) {
direction = 2;
pc.printf("mid ");
wait(TIMEB4NEXTCHOICE); // Tijdelijke wait om paars lampje aan te zetten ter controle selectie
@@ -375,7 +373,7 @@
dir2 =0;
dir3 =1;
for (int lag=0; lag<TIMEBETWEENBLINK; lag++) {
- if(bi_result>0.8*bi_max) {
+ if(bi_result>FACTOR*bi_max) {
direction = 3;
pc.printf("rechts ");
wait(TIMEB4NEXTCHOICE); // Tijdelijke wait om oranje lampje aan te zetten ter controle selectie
@@ -395,12 +393,12 @@
for2=0;
for3=0;
for (int lag=0; lag<TIMEBETWEENBLINK; lag++) {
- if(tri_result>0.8*tri_max) {
+ if(tri_result>FACTOR*tri_max) {
for1 = for2 = for3 = 0;
pc.printf("reset ");
goto directionchoice;
} else {
- if(bi_result>0.8*bi_max) {
+ if(bi_result>FACTOR*bi_max) {
force = 1;
pc.printf("zwak ");
wait(TIMEB4NEXTCHOICE); // Tijdelijke wait om cyaan lampje aan te zetten ter controle selectie
@@ -416,12 +414,12 @@
for2=1;
for3=0;
for (int lag=0; lag<TIMEBETWEENBLINK; lag++) {
- if(tri_result>0.8*tri_max) {
+ if(tri_result>FACTOR*tri_max) {
for1 = for2 = for3 = 0;
pc.printf("reset ");
goto directionchoice;
} else {
- if(bi_result>0.8*bi_max) {
+ if(bi_result>FACTOR*bi_max) {
force = 2;
pc.printf("normaal ");
wait(TIMEB4NEXTCHOICE); // Tijdelijke wait om paars lampje aan te zetten ter controle selectie
@@ -437,12 +435,12 @@
for2=0;
for3=1;
for (int lag=0; lag<TIMEBETWEENBLINK; lag++) {
- if(tri_result>0.8*tri_max) {
+ if(tri_result>FACTOR*tri_max) {
for1 = for2 = for3 = 0;
pc.printf("reset ");
goto directionchoice;
} else {
- if(bi_result>0.8*bi_max) {
+ if(bi_result>FACTOR*bi_max) {
force = 3;
pc.printf("sterk ");
wait(TIMEB4NEXTCHOICE); // Tijdelijke wait om oranje lampje aan te zetten ter controle selectie
@@ -459,13 +457,31 @@
choicesmade:
blink.attach(okay, 0.2);
while(1) {
- if(tri_result>0.8*tri_max) {
+ if(tri_result>FACTOR*tri_max) {
blink.detach();
pc.printf("reset ");
+ switch (direction) {
+ case 1:
+ dir1 = 1;
+ for1 = 1;
+ for2 = for3 = 0;
+ break;
+ case 2:
+ dir2 = 1;
+ for1 = 1;
+ for2 = for3 = 0;
+ break;
+ case 3:
+ dir3 = 1;
+ for1 = 1;
+ for2 = for3 = 0;
+ break;
+ }
+
wait(1); // 1 sec wait, anders reset je meteen ook de biceps keuze
goto forcechoice;
} else {
- if(bi_result>0.8*bi_max && (dir1==1||dir2==1||dir3==1)) {
+ if(bi_result>FACTOR*bi_max && (dir1==1||dir2==1||dir3==1)) {
blink.detach();
log_timer.detach();
goto motorcontrol;
@@ -479,33 +495,28 @@
/* Vanaf hier komt de aansturing van de motor */
- if(direction == 1 && force == 1) { // links zwak
- pc.printf("links zwak ");
- // hier komt setpoint motor 1, setpoint motor2
- }
- if(direction == 1 && force == 2) { // links normaal
- pc.printf("links normaal ");
- }
- if(direction == 1 && force == 3) { // links sterk
- pc.printf("links sterk ");
- }
- if(direction == 2 && force == 1) { // mid zwak
- pc.printf("mid zwak ");
+ switch (direction) {
+ case 1:
+ setpoint2 = 3.14;
+ break;
+ case 2:
+ setpoint2 = 3.14;
+ break;
+ case 3:
+ setpoint2 = 3.14;
+ break;
}
- if(direction == 2 && force == 2) { // mid normaal
- pc.printf("mid normaal ");
- }
- if(direction == 2 && force == 3) { // mid sterk
- pc.printf("mid sterk ");
- }
- if(direction == 3 && force == 1) { // rechts zwak
- pc.printf("rechts zwak ");
- }
- if(direction == 3 && force == 2) { // rechts normaal
- pc.printf("rechts normaal ");
- }
- if(direction == 3 && force == 3) { // rechts sterk
- pc.printf("rechts sterk ");
+
+ switch (force) {
+ case 1:
+ setpoint1 = 8;
+ break;
+ case 2:
+ setpoint1 = 8;
+ break;
+ case 3:
+ setpoint1 = 8;
+ break;
}
while(1) { // loop voor het goed plaatsen van motor2 (batje hoek)
@@ -583,7 +594,7 @@
pwm = Kp3*controlerror + Ki3*integral;
keep_in_range(&pwm, -1,1);
- if(pwm > 0) {
+ if(pwm > 0) {
motor1dir = 1;
} else {
motor1dir = 0; //1 = rechtsom, 0 = linksom