Tobias Berger / Mbed 2 deprecated Robocode

Tobis Programm forked to not destroy your golden files

Dependencies:   mbed

Fork of Robocode by PES 2 - Gruppe 1

source/Grabbing.cpp@116:e03a3692cdf0, 2017-05-10 (annotated)

Committer:
cittecla
Date:
Wed May 10 08:52:04 2017 +0000
Revision:
116:e03a3692cdf0
Parent:
114:720dc5df42a5 
changed grabbing, move in search for brick and small bug fixes

Who changed what in which revision?

UserRevisionLine numberNew contents of line
cittecla 12:91c2e07d2b5b 1 /**
cittecla 12:91c2e07d2b5b 2 * Grabbing function library
cittecla 12:91c2e07d2b5b 3 * Handels grabbing of the LEGO-stones
cittecla 12:91c2e07d2b5b 4 **/
cittecla 12:91c2e07d2b5b 5
cittecla 39:92723f7ea54f 6 #include "Grabbing.h"
cittecla 12:91c2e07d2b5b 7
aeschsim 66:87b615e4a842 8 #define servo4min 280 //unten
aeschsim 65:1f237921216d 9 #define servo4max 495 //oben
aeschsim 84:435ec041fd0d 10 #define servo6min 265 //oben
aeschsim 65:1f237921216d 11 #define servo6max 480 //unter
aeschsim 65:1f237921216d 12 #define servo8min 120 //offen
aeschsim 65:1f237921216d 13 #define servo8max 260 //zu
aeschsim 65:1f237921216d 14 //Servo 4 Ausleger Servo 6 Gelenk Servo 8 Greifer
aeschsim 65:1f237921216d 15
cittecla 93:837a13760026 16 bool init2 = true; // general init state
cittecla 93:837a13760026 17 int i; // general counter
cittecla 93:837a13760026 18 int grabbing_state = 2;
aeschsim 65:1f237921216d 19
cittecla 93:837a13760026 20
cittecla 93:837a13760026 21 int grabbing()
cittecla 93:837a13760026 22 {
aeschsim 99:78d87027c85b 23 //printf("grabbing state: %d\r\n", grabbing_state);
cittecla 93:837a13760026 24 switch (grabbing_state) {
cittecla 93:837a13760026 25 case 2:
cittecla 93:837a13760026 26 if(arm_position_grabbing()) {
cittecla 93:837a13760026 27 grabbing_state = 3;
cittecla 93:837a13760026 28 }
cittecla 93:837a13760026 29 break;
cittecla 93:837a13760026 30 case 3:
PESGruppe1 114:720dc5df42a5 31 move_for_distance(0.15f);
cittecla 93:837a13760026 32 grabbing_state = 4;
cittecla 93:837a13760026 33 break;
cittecla 93:837a13760026 34 case 4:
cittecla 116:e03a3692cdf0 35 if(move_for_distance(0) < 0.075f) {
cittecla 116:e03a3692cdf0 36 close_grabber_half();
cittecla 116:e03a3692cdf0 37 }
cittecla 93:837a13760026 38 if(move_for_distance(0) < 0) {
cittecla 93:837a13760026 39 grabbing_state = 5;
cittecla 93:837a13760026 40 }
cittecla 93:837a13760026 41 break;
cittecla 93:837a13760026 42 case 5:
cittecla 93:837a13760026 43 close_grabber();
cittecla 93:837a13760026 44 wait(0.5f);
cittecla 93:837a13760026 45 grabbing_state = 6;
cittecla 93:837a13760026 46 break;
cittecla 93:837a13760026 47 case 6:
cittecla 93:837a13760026 48 if (get_color() == 0) {
cittecla 93:837a13760026 49 grabbing_state = 9;
cittecla 93:837a13760026 50 } else {
cittecla 93:837a13760026 51 grabbing_state = 7;
cittecla 93:837a13760026 52 }
cittecla 93:837a13760026 53 break;
cittecla 93:837a13760026 54 case 7:
cittecla 93:837a13760026 55 if(arm_position_release()) {
aeschsim 102:dae11a6102d4 56 wait(0.5f);
cittecla 93:837a13760026 57 open_grabber();
cittecla 93:837a13760026 58 grabbing_state = 10;
cittecla 93:837a13760026 59 }
cittecla 93:837a13760026 60 break;
cittecla 93:837a13760026 61 case 9:
cittecla 93:837a13760026 62 open_grabber();
cittecla 93:837a13760026 63 wait(0.05f);
cittecla 93:837a13760026 64 if(arm_position_release()) {
cittecla 93:837a13760026 65 grabbing_state = 10;
cittecla 93:837a13760026 66 }
cittecla 93:837a13760026 67 break;
cittecla 93:837a13760026 68 case 10:
aeschsim 99:78d87027c85b 69 move_for_distance(-0.10f);
cittecla 93:837a13760026 70 grabbing_state = 11;
cittecla 93:837a13760026 71 break;
cittecla 93:837a13760026 72 case 11:
cittecla 93:837a13760026 73 if(move_for_distance(0) < 0) {
cittecla 93:837a13760026 74 grabbing_state = 2;
cittecla 93:837a13760026 75 return 0;
cittecla 93:837a13760026 76 }
cittecla 93:837a13760026 77 break;
cittecla 39:92723f7ea54f 78 }
cittecla 93:837a13760026 79 return 50;
cittecla 93:837a13760026 80 }
cittecla 93:837a13760026 81
cittecla 93:837a13760026 82 bool arm_position_grabbing()
cittecla 93:837a13760026 83 {
cittecla 93:837a13760026 84 if(init2 == true) {
cittecla 93:837a13760026 85 init2 = false;
cittecla 93:837a13760026 86 i = servo4max;
cittecla 93:837a13760026 87 } else {
cittecla 93:837a13760026 88
cittecla 93:837a13760026 89 if(i > servo4min) {
cittecla 93:837a13760026 90 i -=8;
cittecla 93:837a13760026 91 set_servo_position(4, i);
cittecla 93:837a13760026 92 } else {
cittecla 93:837a13760026 93 init2 = true;
cittecla 93:837a13760026 94 return 1; // done
cittecla 93:837a13760026 95 }
cittecla 93:837a13760026 96 if (i < 470) {
aeschsim 65:1f237921216d 97 set_servo_position(6, servo6max);
aeschsim 65:1f237921216d 98 }
aeschsim 65:1f237921216d 99 }
cittecla 93:837a13760026 100 return 0; // recall
aeschsim 65:1f237921216d 101 }
cittecla 93:837a13760026 102
cittecla 93:837a13760026 103 bool arm_position_release()
cittecla 93:837a13760026 104 {
cittecla 93:837a13760026 105 if( init2 == true) {
cittecla 93:837a13760026 106 set_servo_position(4, servo4max);
aeschsim 102:dae11a6102d4 107 wait_ms(300);
cittecla 93:837a13760026 108 i = servo6max;
cittecla 93:837a13760026 109 init2 = false;
cittecla 93:837a13760026 110 } else {
cittecla 93:837a13760026 111 if(i>=servo6min) {
cittecla 93:837a13760026 112 i -=20;
cittecla 93:837a13760026 113 set_servo_position(6, i);
cittecla 93:837a13760026 114 } else {
cittecla 93:837a13760026 115 init2 = true;
cittecla 93:837a13760026 116 return 1; //open grabber
cittecla 93:837a13760026 117 }
aeschsim 84:435ec041fd0d 118 }
cittecla 93:837a13760026 119 return 0; //recall
cittecla 93:837a13760026 120 }
cittecla 93:837a13760026 121
cittecla 93:837a13760026 122 void open_grabber()
cittecla 93:837a13760026 123 {
aeschsim 65:1f237921216d 124 set_servo_position(8, servo8min);
aeschsim 65:1f237921216d 125 }
cittecla 93:837a13760026 126
cittecla 116:e03a3692cdf0 127 void close_grabber_half()
cittecla 116:e03a3692cdf0 128 {
cittecla 116:e03a3692cdf0 129 set_servo_position(8, ((servo8min+servo8max)/2));
cittecla 116:e03a3692cdf0 130 }
cittecla 116:e03a3692cdf0 131
cittecla 93:837a13760026 132 void close_grabber()
cittecla 93:837a13760026 133 {
aeschsim 65:1f237921216d 134 set_servo_position(8, servo8max);
cittecla 93:837a13760026 135
aeschsim 65:1f237921216d 136 }
cittecla 93:837a13760026 137
cittecla 93:837a13760026 138