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
Revision 2:de3bb38dae4e, committed 2015-10-21
- Comitter:
- Wallie117
- Date:
- Wed Oct 21 11:32:48 2015 +0000
- Parent:
- 1:0e1d91965b8e
- Commit message:
- Dit is het script van de robot, er zijn problemen gekomen door while loops in te zetten. Dit verstoord het lezen van het EMG signaal. Het EMG signaal en Encoder signaal moet in de Tickers komen!!
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
--- 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) {