Biorobotics10
Script met een paar limiet toevoegingen en reset van arm als die te ver door draait.
Dependencies: HIDScope MODSERIAL QEI biquadFilter mbed
Fork of
Samenvoegen6
by 
Biorobotics10
Diff: main.cpp
- Revision:
- 2:de3bb38dae4e
- Parent:
- 1:0e1d91965b8e
--- a/main.cpp Tue Oct 20 09:29:32 2015 +0000
+++ b/main.cpp Wed Oct 21 11:32:48 2015 +0000
@@ -20,8 +20,8 @@
//AnalogIn pot(A2);
//AnalogIn pot1(A3);
-QEI wheel2 (D10, D11, NC, 64,QEI::X4_ENCODING); // Function for Encoder1 on the motor1 to the Microcontroller
-QEI wheel1 (D12, D13, NC, 64,QEI::X4_ENCODING); // Function for Encoder2 on the motor2 to the Microcontroller
+QEI wheel1 (D10, D11, NC, 64,QEI::X4_ENCODING); // Function for Encoder1 on the motor1 to the Microcontroller
+QEI wheel2 (D12, D13, NC, 64,QEI::X4_ENCODING); // Function for Encoder2 on the motor2 to the Microcontroller
//**************** OUTPUTS ****************
DigitalOut red(LED_RED); // LED declared for calibration
@@ -33,10 +33,10 @@
DigitalOut magnet(D2);
-DigitalOut motor2direction(D4); // D4 en D5 zijn motor 2 (op het motorshield)
-PwmOut motor2speed(D5);
-DigitalOut motor1direction(D7); // D6 en D7 zijn motor 1 (op het motorshield)
-PwmOut motor1speed(D6);
+DigitalOut motor1direction(D4); // D4 en D5 zijn motor 2 (op het motorshield)
+PwmOut motor1speed(D5);
+DigitalOut motor2direction(D7); // D6 en D7 zijn motor 1 (op het motorshield)
+PwmOut motor2speed(D6);
//**************** FUNCTIONS **************
/* HIDScope scope(4); */ // HIDScope declared with 4 scopes
@@ -101,23 +101,24 @@
bool flag_left = true;
bool flag_right = true;
float pos_board = 0; // The position of the board begins at zero
+int pos_board1 = 0;
float Encoderread2 = 0;
//********* VARIABLES FOR CONTROL 3 ************
-const float minimum_contract = 0.2; // Certain levels for muscle activity to activate motor
-const float medium_contract = 0.5; // "Medium contract muscle"
+const float minimum_contract = 0.4; // Certain levels for muscle activity to activate motor
+const float medium_contract = 0.6; // "Medium contract muscle"
const float maximum_leftbiceps = 0.8; // "Maximum contract muscle"
const float on = 1.0;
const float off = 0.0;
-const float minimum_throw_angle = -5;
+const float minimum_throw_angle = 20;
const float maximum_throw_angle = 110;
float pos_arm = 0;
int pos_arm1 = 0;
float Encoderread1 = 0;
int switch_rondjes = 2;
int rondjes = 0;
-float pos_armrondjes_max = 5;
+float pos_armrondjes_max = 3;
bool problem1 = false;
bool problem2 = false;
@@ -125,7 +126,8 @@
//********* VARIABLES FOR CONTROL 4 ************
float reset_positie = 0; // Belangrijk
-float reset_board = 0;
+int reset_arm = 0;
+int reset_board = 0;
float speedcompensation;
float speedcompensation2;
int t = 5; // aftellen naar nieuw spel
@@ -153,6 +155,8 @@
double cali_array1[2560] = {}; // array to store values in
double cali_array2[2560] = {};
+double cali_array3[512] = {};
+
void hoog_laag_filter()
{
u1 = EMG1;
@@ -296,9 +300,12 @@
if (flag_s) {
Encoderread1 = wheel1.getPulses();
pos_arm = (Encoderread1/((64.0*131.0)/360.0)); // Omrekenen naar graden van arm
+ pos_arm1 = pos_arm;
Encoderread2 = wheel2.getPulses();
pos_board = (Encoderread2/((64.0*131.0)/360.0)); // Omrekenen naar graden van board
+ pos_board1 = pos_board;
+
flag_calibration = false;
}
//************************* FASE SWITCH ******************************************
@@ -307,6 +314,9 @@
j++;
wait(0.01); // Problemen met EMG metingen die te hoog zijn op het begin van script na calibratie. vandaar ff wachten en de sample loop een paar keer doorlopen.
pc.printf("waarde j = %i \n",j);
+ led_rood.write(0);
+ led_groen.write(1);
+ led_geel.write(0);
}
if( j > N) {
@@ -315,37 +325,31 @@
//******************* Fase 1 **************************
case(1):
+ led_rood.write(1);
+ led_groen.write(0);
+ led_geel.write(0);
rondjes = 0;
if (y1> contract) {
flag_right = false;
flag_left = true;
- led_rood.write(0);
- led_groen.write(0);
- led_geel.write(1);
+
}
if (y2 > contract) {
flag_right = true;
flag_left = false;
- led_rood.write(0);
- led_groen.write(0);
- led_geel.write(1);
+
}
if (pos_board < maxleft) {
flag_left = false;
motor2speed.write(relax);
- led_rood.write(1);
- led_groen.write(0);
- led_geel.write(0);
+
}
if (pos_board > maxright) {
flag_right = false;
motor2speed.write(relax);
- led_rood.write(1);
- led_groen.write(0);
- led_geel.write(0);
}
if (flag_left == true) {
@@ -370,14 +374,17 @@
motor2speed.write(relax);
fase = 2;
fase_switch_wait = true;
+ led_rood.write(0);
+ led_groen.write(0);
+ led_geel.write(1);
j = 0;
- led_rood.write(0);
- led_groen.write(1);
- led_geel.write(0);
}
break;
//******************* Fase 2 **************************
case(2):
+ led_rood.write(0);
+ led_groen.write(0);
+ led_geel.write(1);
motor1direction.write(cw);
pos_arm1 = (pos_arm - (rondjes * 360));
@@ -396,30 +403,18 @@
if (y2 > minimum_contract & y2 < medium_contract) {
motor1speed.write((y2 - minimum_contract)/(medium_contract - minimum_contract));
- led_rood.write(0);
- led_groen.write(0);
- led_geel.write(1);
}
if (y2 > medium_contract) { // Hoger dan drempelwaarde = Actief
motor1speed.write(1);
- led_rood.write(0);
- led_groen.write(0);
- led_geel.write(1);
}
if (y2 < minimum_contract) { // Lager dan drempel = Rust
motor1speed.write(relax);
- led_rood.write(0);
- led_groen.write(1);
- led_geel.write(0);
}
if( rondjes == pos_armrondjes_max) { // max aantal draaing gemaakt!!!!!!
problem1 = true;
problem2 = true;
motor1speed.write(relax);
- led_rood.write(1);
- led_groen.write(0);
- led_geel.write(0);
while (problem1) {
j++;
@@ -444,29 +439,21 @@
led_rood.write(0);
wait(0.1);
led_rood.write(1);
- }
+ wait(1.5);
while(problem2) {
- motor1speed.write(relax);
- wait(1.5);
- while(1) {
- led_rood.write(0);
- led_groen.write(1);
- led_geel.write(0);
-
- motor1direction.write(0);
- problem_velocity = 0.05 + (pos_arm/720)*0.4;
- motor1speed.write(problem_velocity);
+ motor1direction.write(ccw);
+ motor1speed.write(0.02);
if (flag_s) {
Encoderread1 = wheel1.getPulses();
pos_arm = (Encoderread1/((64.0*131.0)/360.0)); // Omrekenen naar graden van arm
}
- if (pos_arm < 0) {
- led_rood.write(0);
- led_groen.write(1);
- led_geel.write(0);
+ if (pos_arm < 10) {
+
problem2 = false;
+ motor1speed.write(0);
+ rondjes = 0;
wait(1);
}
}
@@ -478,10 +465,6 @@
fase = 3;
pc.printf("Van fase 2 naar fase 3\n");
- led_rood.write(0);
- led_groen.write(1);
- led_geel.write(0);
-
wait(2);
j = 0;
fase_switch_wait = true;
@@ -492,48 +475,50 @@
break;
//********************************************* Fase 3 **********************************************
case(3):
-
+ led_rood.write(0);
+ led_groen.write(1);
+ led_geel.write(0);
switch(switch_reset) {
case(1):
if(pos_arm < reset_positie) { //ene kant op draaien
motor1direction.write(cw);
- speedcompensation2 = ((reset_positie - pos_arm)/900.0 + (0.1));
- motor1speed.write(speedcompensation2);
- }
- if(pos_arm > reset_positie) { //andere kant op
- motor1direction.write(ccw);
- speedcompensation2 = ((pos_arm - reset_positie)/500.0 + (0.1));
+ speedcompensation2 = 0.05;//((reset_arm - pos_arm1)/900.0 + (0.02));
motor1speed.write(speedcompensation2);
}
- if(pos_arm < reset_positie+3 && pos_arm > reset_positie-3) { // Dit gaat niet goed komen, omdat het precies die waarde moet zijn
- for(int cali_index1 = 0; cali_index1 < 2560; cali_index1++)
-
- motor1speed.write(0);
- arm = true;
- switch_reset = 2;
- }
+ if(pos_arm > reset_positie) //andere kant op
+ {
+ motor1direction.write(ccw);
+ speedcompensation2 = 0.05;//((pos_arm1 - reset_arm)/500.0 + (0.02));
+ motor1speed.write(speedcompensation2);
+ }
+ if(pos_arm < reset_positie+5 && pos_arm > reset_positie-5) // Dit gaat niet goed komen, omdat het precies die waarde moet zijn
+ {
+ motor1speed.write(0);
+ arm = true;
+ switch_reset = 2;
+ }
pc.printf("Positie_arm = %f \t \t snelheid = %f \n",pos_arm, speedcompensation);
wait(0.0001);
break;
case(2):
pc.printf("\t switch magneet automatisch \n");
- wait(1);
+ wait(0.2);
magnet.write(on);
- wait(0.5);
+ wait(2);
switch_reset = 3;
break;
-
+
case(3):
if(pos_board < reset_board) {
motor2direction.write(cw);
- speedcompensation = ((reset_board - pos_board)/500.0 + (0.1));
+ speedcompensation = 0.05; //((reset_board - pos_board1)/500.0 + (0.1));
motor2speed.write(speedcompensation);
}
if(pos_board > reset_board) {
motor2direction.write(ccw);
- speedcompensation = ((pos_board - reset_board)/500.0 + (0.1));
+ speedcompensation = 0.05;//((pos_board1 - reset_board)/500.0 + (0.05));
motor2speed.write(speedcompensation);
}
if(pos_board < reset_board+5 && pos_board > reset_board-5) {