Koen Boulogne
Basis aansturing projectgroep 3
Dependencies: Biquad HIDScope MODSERIAL QEI mbed
Revision 9:e764cb50d343, committed 2016-11-07
- Comitter:
- s1588141
- Date:
- Mon Nov 07 13:30:49 2016 +0000
- Parent:
- 8:656b0c493a45
- Commit message:
- Final program;
Changed in this revision
| main.cpp | Show annotated file Show diff for this revision Revisions of this file |
diff -r 656b0c493a45 -r e764cb50d343 main.cpp
--- a/main.cpp Tue Nov 01 11:18:41 2016 +0000
+++ b/main.cpp Mon Nov 07 13:30:49 2016 +0000
@@ -51,13 +51,12 @@
double EMG_L;
double EMG_R;
double EMG_Change;
-//-------------------Hidscope----------------------------------
//--------------------------------------------------------------
//--------------------All Motors--------------------------------
//--------------------------------------------------------------
-volatile int movement = 0,direction_L =0, direction_R =0; //determining the direction of the motors
+volatile int movement = 1,direction_L =1, direction_R =-1; //determining the direction of the motors
DigitalOut M_L_D(D4); //Richting motor links->
//while M_L_D is zero the motor's direction is counterclockwise and the pulses are positive
@@ -80,11 +79,10 @@
//-----------------------on of sitch of the sytem-----------------------------
-InterruptIn OnOff(SW3); //System On/off
-volatile bool Active = false;
-void Start_Stop(){
- Active = !Active;
- }
+
+
+AnalogIn Active(A5);
+
//--------------------------------Sampling------------------------------------------
@@ -96,7 +94,7 @@
void Set_Sample_Flag(){
Sample_Flag = true;
}
-
+
void Sample(){
//This function reads out the position and the speed of the motors in radiants
@@ -107,7 +105,6 @@
Previous_Position_R = Position_R;
Position_L = Encoder_L.getPulses()*AnglePerPulse+Previous_Position_L;
Position_R = Encoder_R.getPulses()*AnglePerPulse+Previous_Position_R;
-
//Speed in RadPers second
Speed_L = (Position_L-Previous_Position_L)*SampleFrequency;
Speed_R = (Position_R-Previous_Position_R)*SampleFrequency;
@@ -162,15 +159,14 @@
} else {
M_L_D = 0;
}
- if (fabs(Speed_Set)< 0.4){
+ if (fabs(Speed_Set)< 1){
if(fabs(reference-signal*direction) < 0.5 and reference <0.5){ //Applying Setpoint tollerance
return 0;
} else {
return fabs(Speed_Set);
}
} else {
- return 0.4;
-
+ return 1;
}
}
double e_R_int = 0;
@@ -189,14 +185,14 @@
} else {
M_R_D = 1;
}
- if (fabs(Speed_Set)< 0.4){
+ if (fabs(Speed_Set)< 1){
if(fabs(reference-signal*direction) < 0.5 and reference <0.5){ //Applying Setpoint tollerance
return 0;
} else {
return fabs(Speed_Set);
}
} else {
- return 0.4;
+ return 1;
}
}
@@ -215,10 +211,10 @@
if (Movement_Flag == true){
if (EMG_Change > 0.5f ){
check += 1;
- if (check <= 10 and check > 1 ){
+ if (check <= 6 and check > 1 ){
grip1 = 1;
}
- if (check > 10){
+ if (check > 6){
grip2 =1;
}
} else {
@@ -258,16 +254,17 @@
BiQuad bq1_Change( 9.35527e-01, -1.87105e+00, 9.35527e-01, -1.86689e+00, 8.75215e-01 );
BiQuad bq2_Change( 9.90278e-01, -1.40046e+00, 9.90278e-01, -1.40046e+00, 9.80555e-01 );
BiQuad bq3_Change( 2.20594e-05, 4.41189e-05, 2.20594e-05, -1.98667e+00, 9.86760e-01 );
-HIDScope scope(5); // Sending two sets of data
+//HIDScope scope(5); // Sending two sets of data
+HIDScope scope(3);
void getEMG(){
EMG_Change = bqc_Change.step(Change.read());
EMG_Change = fabs(EMG_Change);
- EMG_Change = bq3_Change.step(EMG_Change)*50;
+ EMG_Change = bq3_Change.step(EMG_Change)*30;
scope.set(0,EMG_Change);//=------------------------------------------------------------------------Scope
- scope.set(4,Change.read());
- if (EMG_Change < 0.3){
+ //scope.send(); // scope.set(4,Change.read());
+ if (EMG_Change < 0.4){
EMG_Change=0;
} else {
EMG_Change=1.0;
@@ -276,27 +273,27 @@
EMG_L = bqc_L.step(Left.read());
EMG_L= fabs(EMG_L);
- EMG_L= bq3_L.step( EMG_L)*10;
+ EMG_L= bq3_L.step( EMG_L)*30;
scope.set(1,EMG_L);//=------------------------------------------------------------------------Scope
- if (EMG_L < 0.3){
+ if (EMG_L < 0.5){
EMG_L=0;
} else {
- EMG_L=1.0;
+ //EMG_L=1.0;
}
EMG_R = bqc_R.step(Right.read());
EMG_R= fabs(EMG_R);
- EMG_R= bq3_R.step( EMG_R)*10;
- scope.set(2,EMG_R);//=------------------------------------------------------------------------Scope
- if (EMG_R < 0.3){
+ EMG_R= bq3_R.step( EMG_R)*30;
+ scope.set(2,EMG_R);//=------------------------------------------------------------------------Scope
+ if (EMG_R < 0.5){
EMG_R=0;
} else {
- EMG_R= 1.0;
+ //EMG_R= 1.0;
}
-
+ scope.send();
}
//------------------------------------------------------Postition reset----------------------------------
//After the robot has been set in it's equlibrium state, this data is used to prevent the robot of destroying itself.
@@ -308,20 +305,28 @@
//-------------------------------------------------------Limit-------------------------------------
//calculates the angele of the robot with for its limit, and checks of its limit has been reached
-
+ int config_count = 0;
+ double max_angle = 0;
+ double min_angle = 0.5;
int Limit(){
double angle=Position_R-Position_L; //R moves the left arm and right moves the right arm
int limit = 0;
- if (fabs(angle) < -13*2*pi*136/(33*360) ){ //limit angle of -13 degrees translated to the motor
+ if (angle < min_angle){ //limit angle of -13 degrees translated to the motor -13*2*pi*136/(33*360)
limit = 1;
}
- if (angle > 15*2*pi*136/(33*360) ){ //limit angle of 15 degrees tranlated to the motor
+ if (angle > max_angle){ //limit angle of 15 degrees tranlated to the motor 2*15*2*pi*136/(33*360)
limit = 2;
}
return limit;
}
+
+
+
+bool Configure_Flag = true;
+
+
/////////////-----------------------Booting---------------------------------------=
void Boot(){
@@ -334,22 +339,20 @@
M_L_S = 0.0;
M_R_S.period(1.0/Motor_Frequency);
M_R_S= 0.0;
- rotation = 1;
- translation = 0;
+ rotation = 0;
+ translation = 1;
Encoder_L.reset();
Encoder_R.reset();
- Grap = 1;
- grip1 = 1;
+ Grap = 0;
+ grip1 = 0;
bqc_L.add( &bq1_L ).add( &bq2_L );
bqc_R.add(&bq1_R).add(&bq2_R);
bqc_Change.add(&bq1_Change).add(&bq2_Change);
- Tick_Sample.attach(Set_Sample_Flag, 1.0/SampleFrequency); Tick_Controller.attach(Set_controller_Flag, 1.0/SampleFrequency);
+ Tick_Sample.attach(Set_Sample_Flag, 1.0/SampleFrequency);
+ Tick_Controller.attach(Set_controller_Flag, 1.0/SampleFrequency);
Tick_Movement.attach(Set_movement_Flag, 0.25);
- OnOff.fall(Start_Stop);
-
- int Maxmin;
}
@@ -365,28 +368,75 @@
//\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/\/
while (true) {
if (Sample_Flag == true){
+
Sample();
getEMG();
// EMG_Change = getEMG( bqc.step(Change.read()));
Signal = pi*(EMG_R-EMG_L);
- scope.set(3,Signal);//------------------------------------------------------------scope
- scope.send();
+
+ //Signal = pi*(Right.read()-Left.read());
+ //EMG_Change = Change.read();
+ // scope.set(3,Signal);//------------------------------------------------------------scope
+
+ // scope.send();
Sample_Flag = false;
- }
+ }
+ //-------------------------------------------- Configureren-------------------------------------
+ if (Configure_Flag == true){
+
+ if (config_count < 20){
+ Signal = pi;
+ } else {
+ Signal = -pi;
+ }
+ if (config_count == 19){
+ Position_reset();
+
+ }
+ if (config_count > 40){
+ Configure_Flag = false;
+ max_angle = Position_R-Position_L-0.5;
+ }
+
+
+ M_L_S = 0.02*Controller_L(direction_L,Signal,Speed_L);
+ M_R_S = 0.02*Controller_R(direction_R,Signal,Speed_R);
+
+
+ //Signal = -pi;
+ // M_L_S = 0.03*Controller_L(direction_L,Signal,Speed_L);
+ // M_R_S = 0.03*Controller_R(direction_R,Signal,Speed_R);
+
+
+ Grap = 0;
+ if (Movement_Flag == true){
+ rotation = !rotation;
+ translation =!translation;
+
+ grip1 = !grip1;
+ grip2 = !grip2;
+ config_count ++;
+ Movement_Flag= false;
+ }
+ } else {
//Main statement that states if the system is active or not
- if (Active == true){
+ if (Active.read() > 0.5){
green = 0;
red = 1;
Change_Movement();
+
if (Controller_Flag == true){
switch (movement) {
case 0:
//Clockwise rotation of the robot
- direction_L = -1;
- direction_R = -1;
+ direction_L = 1;
+ direction_R = 1;
rotation = 1;
translation = 0;
+ Signal = Signal*2;
+ M_L_S = Controller_L(direction_L,Signal,Speed_L);
+ M_R_S = Controller_R(direction_R,Signal,Speed_R);
break;
case 1:
@@ -395,36 +445,46 @@
direction_R = -1;
rotation = 0;
translation = 1;
+
+ switch (Limit()){ //making sure the limits are not passed
+ case 0: //case the robot is in its working space
+ M_L_S = Controller_L(direction_L,Signal,Speed_L);
+ M_R_S = Controller_R(direction_R,Signal,Speed_R);
+ break;
+ case 1: //case the robot has reached it's lowest limit
+ if (Signal < 0){
+ M_L_S = Controller_L(direction_L,Signal,Speed_L);
+ M_R_S = Controller_R(direction_R,Signal,Speed_R);
+ } else {
+ M_L_S = 0;
+ M_R_S = 0;
+ }
+ break;
+ case 2: //case the robot has reachted its highest limit
+ if (Signal > 0){
+ M_L_S = Controller_L(direction_L,Signal,Speed_L);
+ M_R_S = Controller_R(direction_R,Signal,Speed_R);
+ } else {
+ M_L_S = 0;
+ M_R_S = 0;
+ }
+ break;
+ }
break;
}
- switch (Limit()){ //setting limit
- case 0:
- M_L_S = Controller_L(direction_L,Signal,Speed_L);
- M_R_S = Controller_R(direction_R,Signal,Speed_R);
- break;
- case 1:
- if (Signal < 0){
- M_L_S = Controller_L(direction_L,Signal,Speed_L);
- M_R_S = Controller_R(direction_R,Signal,Speed_R);
- }
- break;
- case 2:
- if (Signal > 0){
- M_L_S = Controller_L(direction_L,Signal,Speed_L);
- M_R_S = Controller_R(direction_R,Signal,Speed_R);
- }
- break;
- }
+
Controller_Flag = false;
}
} else {
- if (Active == false){
- green = 1;
+ if (Active.read() < 0.5){
+ green = 1;
red = 0;
M_R_S = 0;
M_L_S = 0;
+ Grap=0;
}
}
}
}
+}