zhaw_st16b_pes2_10
x
Dependencies: Servo ServoArm mbed
Fork of
PES_PIXY_Officiall
by 
zhaw_st16b_pes2_10
Diff: main.cpp
- Revision:
- 3:63da1d1fae15
- Parent:
- 2:c8c965d48f8d
- Child:
- 4:d611df1ed42b
diff -r c8c965d48f8d -r 63da1d1fae15 main.cpp
--- a/main.cpp Thu May 25 18:12:28 2017 +0000
+++ b/main.cpp Fri May 26 08:01:48 2017 +0000
@@ -1,12 +1,8 @@
#include "mbed.h"
#include "Robot.h"
#include "Declarations.h"
-#include "Ultraschall.h"
#include "Pixy.h"
#include "PID_Control.h"
-#include "LowpassFilter.h"
-#include "EncoderCounter.h"
-
#include <cstdlib>
//Cam:
@@ -27,10 +23,6 @@
AnalogIn leftS(A2);
AnalogIn rightS(A3);
-
-//Ultraschall
-Ultraschall usensor(D6,D5);
-
//Leds related:
DigitalOut leds[6] = { PC_8, PC_6, PB_12, PA_7, PC_0, PC_9 };
@@ -48,9 +40,6 @@
//Greifer:
Servo servoGreifer(PC_7);
-//Leiste:
-Servo servoLeiste(PB_6);
-
//Button:
DigitalIn mybutton(USER_BUTTON);
@@ -59,318 +48,134 @@
//DigitalOut led(D2);
-Robot sam( &left, &right, &powerSignal, leds, &FarbVoltage, &frontS, &leftS, &rightS, &servoArm, &servoGreifer, &servoLeiste, &usensor, &pixy ); //Implement the Farbsensor into the Robot init function!!
+Robot sam( &left, &right, &powerSignal, leds, &FarbVoltage, &frontS, &leftS, &rightS, &servoArm, &servoGreifer, &pixy ); //Robot-Constructor
-void initializeDistanceSensors()
-{
- for( int ii = 0; ii<9; ++ii) {
+void initializeDistanceSensors(){
+ for( int ii = 0; ii<9; ++ii){
sam.sensors[ii].init(&sensorVoltage, &frontS, &leftS, &rightS, &bit0, &bit1, &bit2, ii);
-
enable = 1;
}
}
-/* */
-int main()
-{
- initializeDistanceSensors(); //Initialises IR-Sensors.
- enum states {search = 0, setPos, take};
- int time = 0; //Time keeps track of time. [time] = ms
+
+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
PID_Control pid;
pid.setPIDValues( 0.001f, 0.001f, 0.00015f, 0.15f, -0.15f, 1000);
- // pc.baud( 115200 );
-
- int state = search;
- while(!sam.Arm.collectToBack())wait(0.075);
- while(!sam.Greifer.leave())wait(0.001);
-
-
- while( 1 ) {
-
- switch(state){
+
+ sam.Arm.init(&servoArm); //Arm initialisation
+ sam.Greifer.init(&servoGreifer); //Greifer initialisation
- case search: {
- if(!((pixy.getX()>50 && pixy.getX()<300)&&(pixy.getY()>50 && pixy.getY()<300))){
- sam.search(&time);
- }
- else {
- state = setPos;
- }
-
- break;
- }
-
- case setPos: {
- sam.leds[5] = 1;
- static int i = 0;
- float eX = 133.0f - pixy.getX();
- float diffX = pid.calc( eX, 0.006f );
-
- float aX = 0.03; //minimum diff
- float aY = 0.05;
- if(diffX>0) {
- if(diffX<aX)diffX=aX;
- }
- else if(diffX>-aX)diffX=-aX;
-
- if(!(pixy.getX()>128 && pixy.getX()<138)){
- sam.setLeft(0.5f - diffX);
- sam.setRight(0.5f - diffX);
- }
-
- wait(0.001f);
-
- float eY = 121.0f - pixy.getY();
- float diffY = pid.calc( eY, 0.006f );
- if(diffY>0) {
- if(diffY<aY)diffY=aY;
- }
- else if(diffY>-aY) diffY=-aY;
- if(!(pixy.getY()>115 && pixy.getY()<125)){
- 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)) {
- state = take;
- i = 0;
- sam.stop();
- sam.leds[5] = 0;
- }
- break;
- }
-
- case take: {
- enum takeStates {down=0, take, up, leave};
- static int tState=down;
-
- sam.leds[1] = 1;
- sam.stop();
- //tState=leave;
-
- switch(tState) {
- case down:
- if(sam.Arm.backToDown()) tState=take;
- else tState=down;
- break;
- case take:
- if(sam.Greifer.take()) tState=up;
- else tState=take;
- break;
- case up:
- if(sam.Arm.downToBack()) {wait(0.05);tState=leave;}
- else tState=up;
- break;
- case leave:
- if(sam.Greifer.leave()) {state=search; tState=down;}
- else tState=leave;
- break;
- }
+ int state = search;
+ while(!sam.Arm.collectToBack())wait(0.075);
+ while(!sam.Greifer.leave())wait(0.001);
+
+ //while(mybutton) wait(0.01);
- sam.leds[1] = 0;
- break;
- }
- }
- time++;
- wait(0.005f);
-}
-
-return 0;
-}
-
-/* * /
-int main()
-{
- initializeDistanceSensors(); //Initialises IR-Sensors.
- //int counter = 0; //Counts how many times the robot has turned, before driving
- int timer = 0; //Is used, to reset the robot. Returns time in [0.1s]
- //int lastFun = -1; //Is used, to check if the same action in Robot::search() was made multiple times.
- //int found = 0; //0:= no block available, 1 := a lego is ready to be picked up
- //int done = 0;
- int color;
-
- enum states { search = 0, LeisteDown, turn, push, backOff, forward, downward, down, upward, colorS, readyDrop, backward, LeisteUp };
-
- int state = search;
-
- static float messung = 0;
-
- while( 1 ){
- printf("\n\r%f", sam.see(FWD_L));
-
- wait(1.0f);
-
- }
-
- while( 1 ) {
-
- if ( timer > TIMEOUT ) {
- NVIC_SystemReset(); //Resets Sam.
- }
-
- //if( timer == 0 )
- //printf("\n\rLEFT: %.3f,\tFWD: %.3f,\tRIGHT: %.3f", sam.sensors[LEFT].read(), sam.sensors[FWD].read(), sam.sensors[RIGHT].read());
-
-
- //printf("\n\rcurrent main state: %d", state);
-
- sam.sensors[FWD_L].read() < NEAR ? sam.leds[1] = 1 : sam.leds[1] = 0;
- switch( state ) {
- case search:
- if( sam.search(&timer) ){
- //sam.Greifer.nullPos();
- state = LeisteDown;
- timer = 0;
- }
- break;
-
- case LeisteDown:
- int count = 0;
- if( sam.Leiste.upToDown() ){
- //sam.Greifer.leave();
- state = turn;
- timer = 0;
- }
- break;
-
- case turn:
- static int i = 0;
- if( i > 7 ){
- sam.stop();
- state = push;
- i = 0;
+ while(1){
+
+ switch(state){
+
+ case search:{
+ if(!((pixy.getX()>50 && pixy.getX()<225)&&(pixy.getY()>50 && pixy.getY()<300))){
+ sam.search(&time);
}
else{
- i++;
- sam.turnRight();
- }
- break;
-
- case push:{
- static int i = 0;
- if( i > 5 ){
- sam.stop();
- i = 0;
- state = backOff;
- timer = 0;
- }
- else{
- sam.driveSlowly();
- i++;
+ state = setPos;
}
break;
}
-
- case backOff:{
- static int i = 0;
- if( i > 1 ){
- sam.stop();
- i = 0;
- state = forward;
- timer = 0;
+
+ case setPos: {
+
+ sam.leds[5] = 1;
+ static int i = 0;
+ if(!((pixy.getX()>50 && pixy.getX()<225)&&(pixy.getY()>50 && pixy.getY()<300))){
+ state=search;
+ i=0;
+ }
+ float eX = 133.0f - pixy.getX();
+ float diffX = pid.calc( eX, 0.006f );
+
+ float aX = 0.03; //minimum diff X
+ float aY = 0.05; //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)){
+ sam.setLeft(0.5f - diffX);
+ sam.setRight(0.5f - diffX);
+ }
+
+ wait(0.001f);
+
+ float eY = 121.0f - pixy.getY();
+ float diffY = pid.calc( eY, 0.006f );
+ if(diffY>0) {
+ if(diffY<aY)diffY=aY;
+ }
+ else if(diffY>-aY)diffY=-aY;
+
+ //Set the Y position
+ if(!(pixy.getY()>115 && pixy.getY()<125)){
+ 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)) {
+ state = take;
+ i = 0;
+ sam.stop();
+ sam.leds[5] = 0;
+ }
+ break;
+ }
+
+ case take:{
+ 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;
+ else tState=down;
+ break;
+ case take:
+ if(sam.Greifer.take()) tState=up;
+ else tState=take;
+ break;
+ case up:
+ if(sam.Arm.downToBack()) {wait(0.1);tState=leave;}
+ else tState=up;
+ break;
+ case leave:
+ if(sam.Greifer.leave()){
+ state=search;
+ tState=down;
+ }
+ else tState=leave;
+ break;
}
- else{
- sam.driveBackSlowly();
- i++;
- }
+ sam.leds[1] = 0;
break;
}
-
-
- case forward:
- if( sam.Arm.backToCollect() ){
- state = downward;
- timer = 0;
- }
- break;
-
- case downward:
- if( sam.Arm.collectToDown() ){
- state = down;
- timer = 0;
- }
- break;
-
- case down:
- if( sam.Greifer.take() ) {
- state = upward;
- timer = 0;
- }
- break;
-
- case upward:
- if( sam.Arm.downToCollect() ){
- state = colorS;
- timer = 0;
- }
- break;
-
- case colorS:
- led = 1;
- color = sam.FarbVoltage.read();
-
- if( color == -1 ){
- //Do nothing
- }
-
-
- else if( color == 0 || color == GREEN ){
- state = backward;
- led = 0;
- timer = 0;
- }
-
- else if( color == RED ){
- state = readyDrop;
- led = 0;
- timer = 0;
- }
-
- else{
- //Shit...
- }
- break;
-
- case readyDrop:
- if( sam.Greifer.leave() ){
- if( color == GREEN || color == 0 ){
- state = LeisteUp;
- }
- else{
- state = backward;
- }
- timer = 0;
- }
-
- break;
-
- case backward:
- if( sam.Arm.collectToBack() ){
- //sam.Greifer.nullPos();
- state = LeisteUp;
- timer = 0;
- if( color == GREEN || color == 0 ){
- state = readyDrop;
- sam.Greifer.leave();
- }
- else{
- state = LeisteUp;
- }
- }
- break;
-
- case LeisteUp:
- if( sam.Leiste.downToUp() ){
- state = search;
- timer = 0;
- }
- break;
}
- timer++;
+ time++;
+ wait(0.005f); //Cycle-period
+ }
+}
+int main(){
+ while( 1 ){
+ if( !mybutton ) competition();
wait(0.1f);
}
-
- return 0;
-}
-/* */
\ No newline at end of file
+}
\ No newline at end of file