x
Dependencies: Servo ServoArm mbed
Fork of PES_PIXY_Officiall by
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?
User | Revision | Line number | New 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 | } |