zhaw_st16b_pes2_10
x
Dependencies: Servo ServoArm mbed
Fork of
PES_PIXY_Officiall
by 
zhaw_st16b_pes2_10
Revision 5:acb938f45b9c, committed 2017-05-26
- Comitter:
- EpicG10
- Date:
- Fri May 26 13:43:19 2017 +0000
- Parent:
- 4:d611df1ed42b
- Commit message:
- c
Changed in this revision
--- a/Headers/Declarations.h Fri May 26 08:24:59 2017 +0000 +++ b/Headers/Declarations.h Fri May 26 13:43:19 2017 +0000 @@ -3,18 +3,10 @@ //DistanceSensors: #define NEAR 0.18f //Used for distance Sensors. If they're to near to a wall -> turn -#define NEAR_LEGO 0.12f //If the DistanceSensors are near to a Lego... - -#define LEFT_L 5 //Arrayindex of the left LEGO Sensor & left LED -#define FWD_L 0 //Arrayindex of the front LEGO Sensor & front LED -#define RIGHT_L 1 //Arrayindex of the right LEGO Sensor & right LED #define LEFT 7 //Arrayindex of the left Sensor #define FWD 6 //Arrayindex of the front Sensor #define RIGHT 8 //Arrayindex of the right Sensor -#define BRIGHT 2 //Arrayindex of the backward right Sensor & left LED -#define BWD 3 //Arrayindex of the backward Sensor & front LED -#define BLEFT 4 //Arrayindex of the backsward left Sensor & right LED //ColorSensors: #define RED_UPLIMIT 1400 //Default limit in mV @@ -25,19 +17,11 @@ #define NOBLOCK 0 // #define RED 2 // -//Greifer: -//#define PC_7 SERVO0 - //Arm #define COLLECT_POS 60.0f #define RELEASE_POS 87.0f #define TAKE_POS -65.0f - -// -#define UP_POS 20.0f //zu bestimmen -#define DOWN_POS -72.0f //zu bestimmen - //Misc: #define TIMEOUT 3000 //if the timer reaches TIMEOUT ([TIMEOUT] = 0.1s), the robot will reset. #define MAX 3000 //Once the counter reaches MAX, it will turn around.
--- a/Sources/Arm.cpp Fri May 26 08:24:59 2017 +0000
+++ b/Sources/Arm.cpp Fri May 26 13:43:19 2017 +0000
@@ -12,14 +12,14 @@
void Arm::init(ServoArm* arm){
this->arm = arm;
this->arm->calibrate(0.0015f, 180.0f);
- this->arm->position(RELEASE_POS);
+ this->arm->position(RELEASE_POS+10.0f);
}
int Arm::collectToBack(){
static float i=0;
- if(i<5){
- this->arm->position(85.0);
+ if(i<10){
+ this->arm->position(RELEASE_POS+10.0f);
i++;
return 0;
}
@@ -47,7 +47,7 @@
static float pos = TAKE_POS;
static int i=0;
if( pos < RELEASE_POS ){
- pos += 2.0;
+ pos += 2.0f;
this->arm->position(pos);
i++;
return 0;
--- a/Sources/Greifer.cpp Fri May 26 08:24:59 2017 +0000
+++ b/Sources/Greifer.cpp Fri May 26 13:43:19 2017 +0000
@@ -18,8 +18,8 @@
}
int Greifer::take(){
+ static int time = 0;
this->greifer->position(-60.0f);
- static int time = 0;
if( time < 300 ){
time++;
return 0;
@@ -31,8 +31,8 @@
}
int Greifer::leave(){
+ static int time = 0;
this->greifer->position(65.0f);
- static int time = 0;
if( time < 350 ){
time++;
return 0;
--- a/main.cpp Fri May 26 08:24:59 2017 +0000
+++ b/main.cpp Fri May 26 13:43:19 2017 +0000
@@ -60,27 +60,26 @@
void competition(){
initializeDistanceSensors(); //Initialises IR-Sensors
enum states {search = 0, setPos, take}; //3-states machine
- int time = 0; //Time keeps track of time. [time] = ms
+ int time = 0; //Time keeps track of time
PID_Control pid;
pid.setPIDValues( 0.001f, 0.001f, 0.00015f, 0.15f, -0.15f, 1000);
- sam.Arm.init(&servoArm); //Arm initialisation
- sam.Greifer.init(&servoGreifer); //Greifer initialisation
+
int state = search;
+ sam.Arm.init(&servoArm);
while(!sam.Arm.collectToBack())wait(0.075f);
while(!sam.Greifer.leave())wait(0.001f);
-
- //while(mybutton) wait(0.01);
while(1){
+ if(time>TIMEOUT)return;
switch(state){
case search:{
- if(!((pixy.getX()>50 && pixy.getX()<225)&&(pixy.getY()>50 && pixy.getY()<300))){
+ if(!((pixy.getX()>50 && pixy.getX()<250)&&(pixy.getY()>50 && pixy.getY()<300))){
sam.search(&time);
}
else{
@@ -91,25 +90,26 @@
}
case setPos: {
-
+ time++;
sam.leds[5] = 1;
static int i = 0;
- if(!((pixy.getX()>50 && pixy.getX()<225)&&(pixy.getY()>50 && pixy.getY()<300))){
+ if(!((pixy.getX()>50 && pixy.getX()<250)&&(pixy.getY()>50 && pixy.getY()<300))){
+ time=0;
state=search;
i=0;
}
- float eX = 133.0f - pixy.getX();
+ float eX = 131.0f - pixy.getX();
float diffX = pid.calc( eX, 0.006f );
- float aX = 0.03; //minimum diff X
- float aY = 0.05; //minimum diff Y
+ float aX = 0.04; //minimum diff X
+ float aY = 0.04; //minimum diff Y
if(diffX>0) {
if(diffX<aX)diffX=aX;
}
else if(diffX>-aX)diffX=-aX;
//Set the X position
- if(!(pixy.getX()>128 && pixy.getX()<138)){
+ if(!(pixy.getX()>130 && pixy.getX()<133)){
sam.setLeft(0.5f - diffX);
sam.setRight(0.5f - diffX);
}
@@ -124,12 +124,12 @@
else if(diffY>-aY)diffY=-aY;
//Set the Y position
- if(!(pixy.getY()>115 && pixy.getY()<125)){
+ if(!(pixy.getY()>119 && pixy.getY()<121)){
sam.setLeft(0.5f + diffY);
sam.setRight(0.5f - diffY);
}
i++;
- if((pixy.getX()>132 && pixy.getX()<134)&&(pixy.getY()>119 && pixy.getY()<122)||!(i%1000)) {
+ if((pixy.getX()>130 && pixy.getX()<132)&&(pixy.getY()>120 && pixy.getY()<122)||(!(i%1000))) {
state = take;
i = 0;
sam.stop();
@@ -139,15 +139,19 @@
}
case take:{
+ if(time>TIMEOUT)return;
enum takeStates {down=0, take, up, leave}; //4-states machine
static int tState=down;
sam.leds[1] = 1;
sam.stop();
-
+
switch(tState){
case down:
- if(sam.Arm.backToDown()) tState=take;
+ if(sam.Arm.backToDown()){
+ sam.Greifer.leave();
+ tState=take;
+ }
else tState=down;
break;
case take:
@@ -155,7 +159,7 @@
else tState=take;
break;
case up:
- if(sam.Arm.downToBack()) {wait(0.1);tState=leave;}
+ if(sam.Arm.downToBack()) tState=leave;
else tState=up;
break;
case leave:
@@ -177,8 +181,15 @@
int main(){
int start = 0;
while( 1 ){
- if( !mybutton ) start++;
- if( start ) competition();
- wait(0.1f);
+ if( !mybutton ){
+ start++;
+ sam.Arm.init(&servoArm); //Arm initialisation
+ sam.Greifer.init(&servoGreifer); //Greifer initialisation
+ }
+ if( start ){
+ competition();
+ sam.stop();
+ }
+ wait(0.1f);
}
}
\ No newline at end of file