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@93:837a13760026, 2017-05-02 (annotated)

Committer:
cittecla
Date:
Tue May 02 13:25:56 2017 +0000
Revision:
93:837a13760026
Parent:
84:435ec041fd0d
Child:
99:78d87027c85b
Grabbing state machine is now working

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 {
cittecla 93:837a13760026 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:
cittecla 93:837a13760026 31 move_for_distance(0.05f);
cittecla 93:837a13760026 32 grabbing_state = 4;
cittecla 93:837a13760026 33 break;
cittecla 93:837a13760026 34 case 4:
cittecla 93:837a13760026 35 if(move_for_distance(0) < 0) {
cittecla 93:837a13760026 36 grabbing_state = 5;
cittecla 93:837a13760026 37 }
cittecla 93:837a13760026 38 break;
cittecla 93:837a13760026 39 case 5:
cittecla 93:837a13760026 40 close_grabber();
cittecla 93:837a13760026 41 wait(0.5f);
cittecla 93:837a13760026 42 grabbing_state = 6;
cittecla 93:837a13760026 43 break;
cittecla 93:837a13760026 44 case 6:
cittecla 93:837a13760026 45 if (get_color() == 0) {
cittecla 93:837a13760026 46 grabbing_state = 9;
cittecla 93:837a13760026 47 } else {
cittecla 93:837a13760026 48 grabbing_state = 7;
cittecla 93:837a13760026 49 }
cittecla 93:837a13760026 50 break;
cittecla 93:837a13760026 51 case 7:
cittecla 93:837a13760026 52 if(arm_position_release()) {
cittecla 93:837a13760026 53 wait(0.15f);
cittecla 93:837a13760026 54 open_grabber();
cittecla 93:837a13760026 55 grabbing_state = 10;
cittecla 93:837a13760026 56 }
cittecla 93:837a13760026 57 break;
cittecla 93:837a13760026 58 case 9:
cittecla 93:837a13760026 59 open_grabber();
cittecla 93:837a13760026 60 wait(0.05f);
cittecla 93:837a13760026 61 if(arm_position_release()) {
cittecla 93:837a13760026 62 grabbing_state = 10;
cittecla 93:837a13760026 63 }
cittecla 93:837a13760026 64 break;
cittecla 93:837a13760026 65 case 10:
cittecla 93:837a13760026 66 move_for_distance(-0.05f);
cittecla 93:837a13760026 67 grabbing_state = 11;
cittecla 93:837a13760026 68 break;
cittecla 93:837a13760026 69 case 11:
cittecla 93:837a13760026 70 if(move_for_distance(0) < 0) {
cittecla 93:837a13760026 71 grabbing_state = 2;
cittecla 93:837a13760026 72 return 0;
cittecla 93:837a13760026 73 }
cittecla 93:837a13760026 74 break;
cittecla 39:92723f7ea54f 75 }
cittecla 93:837a13760026 76 return 50;
cittecla 93:837a13760026 77 }
cittecla 93:837a13760026 78
cittecla 93:837a13760026 79 bool arm_position_grabbing()
cittecla 93:837a13760026 80 {
cittecla 93:837a13760026 81 if(init2 == true) {
cittecla 93:837a13760026 82 init2 = false;
cittecla 93:837a13760026 83 i = servo4max;
cittecla 93:837a13760026 84 } else {
cittecla 93:837a13760026 85
cittecla 93:837a13760026 86 if(i > servo4min) {
cittecla 93:837a13760026 87 i -=8;
cittecla 93:837a13760026 88 set_servo_position(4, i);
cittecla 93:837a13760026 89 } else {
cittecla 93:837a13760026 90 init2 = true;
cittecla 93:837a13760026 91 return 1; // done
cittecla 93:837a13760026 92 }
cittecla 93:837a13760026 93 if (i < 470) {
aeschsim 65:1f237921216d 94 set_servo_position(6, servo6max);
aeschsim 65:1f237921216d 95 }
aeschsim 65:1f237921216d 96 }
cittecla 93:837a13760026 97 return 0; // recall
aeschsim 65:1f237921216d 98 }
cittecla 93:837a13760026 99
cittecla 93:837a13760026 100 bool arm_position_release()
cittecla 93:837a13760026 101 {
cittecla 93:837a13760026 102 if( init2 == true) {
cittecla 93:837a13760026 103 set_servo_position(4, servo4max);
cittecla 93:837a13760026 104 wait_ms(50);
cittecla 93:837a13760026 105 i = servo6max;
cittecla 93:837a13760026 106 init2 = false;
cittecla 93:837a13760026 107 } else {
cittecla 93:837a13760026 108 if(i>=servo6min) {
cittecla 93:837a13760026 109 i -=20;
cittecla 93:837a13760026 110 set_servo_position(6, i);
cittecla 93:837a13760026 111 } else {
cittecla 93:837a13760026 112 init2 = true;
cittecla 93:837a13760026 113 return 1; //open grabber
cittecla 93:837a13760026 114 }
aeschsim 84:435ec041fd0d 115 }
cittecla 93:837a13760026 116 return 0; //recall
cittecla 93:837a13760026 117 }
cittecla 93:837a13760026 118
cittecla 93:837a13760026 119 void open_grabber()
cittecla 93:837a13760026 120 {
aeschsim 65:1f237921216d 121 set_servo_position(8, servo8min);
aeschsim 65:1f237921216d 122 }
cittecla 93:837a13760026 123
cittecla 93:837a13760026 124 void close_grabber()
cittecla 93:837a13760026 125 {
aeschsim 65:1f237921216d 126 set_servo_position(8, servo8max);
cittecla 93:837a13760026 127
aeschsim 65:1f237921216d 128 }
cittecla 93:837a13760026 129
cittecla 93:837a13760026 130