zhaw_st16b_pes2_10 / Mbed 2 deprecated PES_PIXY_Official_Richtig

x

Dependencies:   Servo ServoArm mbed

Fork of PES_PIXY_Officiall by zhaw_st16b_pes2_10

main.cpp@5:acb938f45b9c, 2017-05-26 (annotated)

Committer:
EpicG10
Date:
Fri May 26 13:43:19 2017 +0000
Revision:
5:acb938f45b9c
Parent:
4:d611df1ed42b 
c

Who changed what in which revision?

UserRevisionLine numberNew contents of line
beacon 0:15a8480061e8 1 #include "mbed.h"
beacon 0:15a8480061e8 2 #include "Robot.h"
beacon 0:15a8480061e8 3 #include "Declarations.h"
beacon 0:15a8480061e8 4 #include "Pixy.h"
beacon 0:15a8480061e8 5 #include "PID_Control.h"
beacon 0:15a8480061e8 6 #include <cstdlib>
beacon 0:15a8480061e8 7
beacon 0:15a8480061e8 8 //Cam:
beacon 0:15a8480061e8 9 Serial cam(PA_9, PA_10);
beacon 0:15a8480061e8 10 Pixy pixy(cam);
beacon 0:15a8480061e8 11
beacon 0:15a8480061e8 12 //DistanceSensors related bottom:
EpicG10 2:c8c965d48f8d 13 Serial pc(SERIAL_TX, SERIAL_RX);
beacon 0:15a8480061e8 14
beacon 0:15a8480061e8 15 AnalogIn sensorVoltage(PB_1);
beacon 0:15a8480061e8 16 DigitalOut enable(PC_1);
beacon 0:15a8480061e8 17 DigitalOut bit0(PH_1);
beacon 0:15a8480061e8 18 DigitalOut bit1(PC_2);
beacon 0:15a8480061e8 19 DigitalOut bit2(PC_3);
beacon 0:15a8480061e8 20
beacon 0:15a8480061e8 21 //DistanceSensors top:
beacon 0:15a8480061e8 22 AnalogIn frontS(A1);
beacon 0:15a8480061e8 23 AnalogIn leftS(A2);
beacon 0:15a8480061e8 24 AnalogIn rightS(A3);
beacon 0:15a8480061e8 25
beacon 0:15a8480061e8 26 //Leds related:
beacon 0:15a8480061e8 27 DigitalOut leds[6] = { PC_8, PC_6, PB_12, PA_7, PC_0, PC_9 };
beacon 0:15a8480061e8 28
beacon 0:15a8480061e8 29 //motor related:
beacon 0:15a8480061e8 30 PwmOut left(PA_8);
beacon 0:15a8480061e8 31 PwmOut right(PA_9);
beacon 0:15a8480061e8 32
beacon 0:15a8480061e8 33 DigitalOut powerSignal(PB_2);
beacon 0:15a8480061e8 34 DigitalIn errorMotor(PB_14);
beacon 0:15a8480061e8 35 DigitalIn overtemperatur(PB_15);
beacon 0:15a8480061e8 36
beacon 0:15a8480061e8 37 //Arm:
beacon 0:15a8480061e8 38 ServoArm servoArm(PA_6);
beacon 0:15a8480061e8 39
beacon 0:15a8480061e8 40 //Greifer:
beacon 0:15a8480061e8 41 Servo servoGreifer(PC_7);
beacon 0:15a8480061e8 42
beacon 0:15a8480061e8 43 //Button:
beacon 0:15a8480061e8 44 DigitalIn mybutton(USER_BUTTON);
beacon 0:15a8480061e8 45
beacon 0:15a8480061e8 46 //Farbsensor:
beacon 0:15a8480061e8 47 AnalogIn FarbVoltage(A0);
beacon 0:15a8480061e8 48 //DigitalOut led(D2);
beacon 0:15a8480061e8 49
beacon 0:15a8480061e8 50
EpicG10 3:63da1d1fae15 51 Robot sam( &left, &right, &powerSignal, leds, &FarbVoltage, &frontS, &leftS, &rightS, &servoArm, &servoGreifer, &pixy ); //Robot-Constructor
beacon 0:15a8480061e8 52
EpicG10 3:63da1d1fae15 53 void initializeDistanceSensors(){
EpicG10 3:63da1d1fae15 54 for( int ii = 0; ii<9; ++ii){
beacon 0:15a8480061e8 55 sam.sensors[ii].init(&sensorVoltage, &frontS, &leftS, &rightS, &bit0, &bit1, &bit2, ii);
beacon 0:15a8480061e8 56 enable = 1;
beacon 0:15a8480061e8 57 }
beacon 0:15a8480061e8 58 }
EpicG10 3:63da1d1fae15 59
EpicG10 3:63da1d1fae15 60 void competition(){
EpicG10 3:63da1d1fae15 61 initializeDistanceSensors(); //Initialises IR-Sensors
EpicG10 3:63da1d1fae15 62 enum states {search = 0, setPos, take}; //3-states machine
EpicG10 5:acb938f45b9c 63 int time = 0; //Time keeps track of time
EpicG10 2:c8c965d48f8d 64
beacon 0:15a8480061e8 65 PID_Control pid;
EpicG10 2:c8c965d48f8d 66 pid.setPIDValues( 0.001f, 0.001f, 0.00015f, 0.15f, -0.15f, 1000);
EpicG10 3:63da1d1fae15 67
EpicG10 5:acb938f45b9c 68
beacon 0:15a8480061e8 69
EpicG10 3:63da1d1fae15 70 int state = search;
EpicG10 5:acb938f45b9c 71 sam.Arm.init(&servoArm);
EpicG10 4:d611df1ed42b 72 while(!sam.Arm.collectToBack())wait(0.075f);
EpicG10 4:d611df1ed42b 73 while(!sam.Greifer.leave())wait(0.001f);
EpicG10 2:c8c965d48f8d 74
EpicG10 2:c8c965d48f8d 75
EpicG10 3:63da1d1fae15 76 while(1){
EpicG10 5:acb938f45b9c 77 if(time>TIMEOUT)return;
EpicG10 3:63da1d1fae15 78
EpicG10 3:63da1d1fae15 79 switch(state){
EpicG10 3:63da1d1fae15 80
EpicG10 3:63da1d1fae15 81 case search:{
EpicG10 5:acb938f45b9c 82 if(!((pixy.getX()>50 && pixy.getX()<250)&&(pixy.getY()>50 && pixy.getY()<300))){
EpicG10 3:63da1d1fae15 83 sam.search(&time);
beacon 0:15a8480061e8 84 }
beacon 0:15a8480061e8 85 else{
EpicG10 3:63da1d1fae15 86 state = setPos;
EpicG10 4:d611df1ed42b 87 time = 0;
beacon 0:15a8480061e8 88 }
beacon 0:15a8480061e8 89 break;
beacon 0:15a8480061e8 90 }
EpicG10 3:63da1d1fae15 91
EpicG10 3:63da1d1fae15 92 case setPos: {
EpicG10 5:acb938f45b9c 93 time++;
EpicG10 3:63da1d1fae15 94 sam.leds[5] = 1;
EpicG10 3:63da1d1fae15 95 static int i = 0;
EpicG10 5:acb938f45b9c 96 if(!((pixy.getX()>50 && pixy.getX()<250)&&(pixy.getY()>50 && pixy.getY()<300))){
EpicG10 5:acb938f45b9c 97 time=0;
EpicG10 3:63da1d1fae15 98 state=search;
EpicG10 3:63da1d1fae15 99 i=0;
EpicG10 3:63da1d1fae15 100 }
EpicG10 5:acb938f45b9c 101 float eX = 131.0f - pixy.getX();
EpicG10 3:63da1d1fae15 102 float diffX = pid.calc( eX, 0.006f );
EpicG10 3:63da1d1fae15 103
EpicG10 5:acb938f45b9c 104 float aX = 0.04; //minimum diff X
EpicG10 5:acb938f45b9c 105 float aY = 0.04; //minimum diff Y
EpicG10 3:63da1d1fae15 106 if(diffX>0) {
EpicG10 3:63da1d1fae15 107 if(diffX<aX)diffX=aX;
EpicG10 3:63da1d1fae15 108 }
EpicG10 3:63da1d1fae15 109 else if(diffX>-aX)diffX=-aX;
EpicG10 3:63da1d1fae15 110
EpicG10 3:63da1d1fae15 111 //Set the X position
EpicG10 5:acb938f45b9c 112 if(!(pixy.getX()>130 && pixy.getX()<133)){
EpicG10 3:63da1d1fae15 113 sam.setLeft(0.5f - diffX);
EpicG10 3:63da1d1fae15 114 sam.setRight(0.5f - diffX);
EpicG10 3:63da1d1fae15 115 }
EpicG10 3:63da1d1fae15 116
EpicG10 3:63da1d1fae15 117 wait(0.001f);
EpicG10 3:63da1d1fae15 118
EpicG10 3:63da1d1fae15 119 float eY = 121.0f - pixy.getY();
EpicG10 3:63da1d1fae15 120 float diffY = pid.calc( eY, 0.006f );
EpicG10 3:63da1d1fae15 121 if(diffY>0) {
EpicG10 3:63da1d1fae15 122 if(diffY<aY)diffY=aY;
EpicG10 3:63da1d1fae15 123 }
EpicG10 3:63da1d1fae15 124 else if(diffY>-aY)diffY=-aY;
EpicG10 3:63da1d1fae15 125
EpicG10 3:63da1d1fae15 126 //Set the Y position
EpicG10 5:acb938f45b9c 127 if(!(pixy.getY()>119 && pixy.getY()<121)){
EpicG10 3:63da1d1fae15 128 sam.setLeft(0.5f + diffY);
EpicG10 3:63da1d1fae15 129 sam.setRight(0.5f - diffY);
EpicG10 3:63da1d1fae15 130 }
EpicG10 3:63da1d1fae15 131 i++;
EpicG10 5:acb938f45b9c 132 if((pixy.getX()>130 && pixy.getX()<132)&&(pixy.getY()>120 && pixy.getY()<122)||(!(i%1000))) {
EpicG10 3:63da1d1fae15 133 state = take;
EpicG10 3:63da1d1fae15 134 i = 0;
EpicG10 3:63da1d1fae15 135 sam.stop();
EpicG10 3:63da1d1fae15 136 sam.leds[5] = 0;
EpicG10 3:63da1d1fae15 137 }
EpicG10 3:63da1d1fae15 138 break;
EpicG10 3:63da1d1fae15 139 }
EpicG10 3:63da1d1fae15 140
EpicG10 3:63da1d1fae15 141 case take:{
EpicG10 5:acb938f45b9c 142 if(time>TIMEOUT)return;
EpicG10 3:63da1d1fae15 143 enum takeStates {down=0, take, up, leave}; //4-states machine
EpicG10 3:63da1d1fae15 144 static int tState=down;
EpicG10 3:63da1d1fae15 145
EpicG10 3:63da1d1fae15 146 sam.leds[1] = 1;
EpicG10 3:63da1d1fae15 147 sam.stop();
EpicG10 5:acb938f45b9c 148
EpicG10 3:63da1d1fae15 149 switch(tState){
EpicG10 3:63da1d1fae15 150 case down:
EpicG10 5:acb938f45b9c 151 if(sam.Arm.backToDown()){
EpicG10 5:acb938f45b9c 152 sam.Greifer.leave();
EpicG10 5:acb938f45b9c 153 tState=take;
EpicG10 5:acb938f45b9c 154 }
EpicG10 3:63da1d1fae15 155 else tState=down;
EpicG10 3:63da1d1fae15 156 break;
EpicG10 3:63da1d1fae15 157 case take:
EpicG10 3:63da1d1fae15 158 if(sam.Greifer.take()) tState=up;
EpicG10 3:63da1d1fae15 159 else tState=take;
EpicG10 3:63da1d1fae15 160 break;
EpicG10 3:63da1d1fae15 161 case up:
EpicG10 5:acb938f45b9c 162 if(sam.Arm.downToBack()) tState=leave;
EpicG10 3:63da1d1fae15 163 else tState=up;
EpicG10 3:63da1d1fae15 164 break;
EpicG10 3:63da1d1fae15 165 case leave:
EpicG10 3:63da1d1fae15 166 if(sam.Greifer.leave()){
EpicG10 3:63da1d1fae15 167 state=search;
EpicG10 3:63da1d1fae15 168 tState=down;
EpicG10 3:63da1d1fae15 169 }
EpicG10 3:63da1d1fae15 170 else tState=leave;
EpicG10 3:63da1d1fae15 171 break;
beacon 0:15a8480061e8 172 }
EpicG10 3:63da1d1fae15 173 sam.leds[1] = 0;
beacon 0:15a8480061e8 174 break;
beacon 0:15a8480061e8 175 }
EpicG10 2:c8c965d48f8d 176 }
EpicG10 3:63da1d1fae15 177 time++;
EpicG10 3:63da1d1fae15 178 wait(0.005f); //Cycle-period
EpicG10 3:63da1d1fae15 179 }
EpicG10 3:63da1d1fae15 180 }
EpicG10 3:63da1d1fae15 181 int main(){
EpicG10 4:d611df1ed42b 182 int start = 0;
EpicG10 3:63da1d1fae15 183 while( 1 ){
EpicG10 5:acb938f45b9c 184 if( !mybutton ){
EpicG10 5:acb938f45b9c 185 start++;
EpicG10 5:acb938f45b9c 186 sam.Arm.init(&servoArm); //Arm initialisation
EpicG10 5:acb938f45b9c 187 sam.Greifer.init(&servoGreifer); //Greifer initialisation
EpicG10 5:acb938f45b9c 188 }
EpicG10 5:acb938f45b9c 189 if( start ){
EpicG10 5:acb938f45b9c 190 competition();
EpicG10 5:acb938f45b9c 191 sam.stop();
EpicG10 5:acb938f45b9c 192 }
EpicG10 5:acb938f45b9c 193 wait(0.1f);
beacon 0:15a8480061e8 194 }
EpicG10 3:63da1d1fae15 195 }