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/Movement.cpp@52:56399c2f13cd, 2017-04-18 (annotated)

Committer:
cittecla
Date:
Tue Apr 18 11:50:02 2017 +0000
Revision:
52:56399c2f13cd
Parent:
39:92723f7ea54f
Child:
53:453f24775644
movement update;

Who changed what in which revision?

UserRevisionLine numberNew contents of line
cittecla 18:a82994e67297 1 /**
cittecla 18:a82994e67297 2 * Movement function library
cittecla 18:a82994e67297 3 * Handels Movement of the Robot
cittecla 18:a82994e67297 4 **/
cittecla 39:92723f7ea54f 5
cittecla 39:92723f7ea54f 6 #include "Movement.h"
cittecla 39:92723f7ea54f 7
cittecla 52:56399c2f13cd 8 int moving()
cittecla 52:56399c2f13cd 9 {
cittecla 52:56399c2f13cd 10
cittecla 39:92723f7ea54f 11 return 0;
cittecla 52:56399c2f13cd 12 }
cittecla 52:56399c2f13cd 13
cittecla 52:56399c2f13cd 14 int move_forward_for_distance(float distance)
cittecla 52:56399c2f13cd 15 {
cittecla 52:56399c2f13cd 16
cittecla 39:92723f7ea54f 17 return 0;
cittecla 52:56399c2f13cd 18 }
cittecla 52:56399c2f13cd 19
cittecla 52:56399c2f13cd 20 int move_backward_for_distance()
cittecla 52:56399c2f13cd 21 {
cittecla 52:56399c2f13cd 22
cittecla 39:92723f7ea54f 23 return 0;
cittecla 52:56399c2f13cd 24 }
cittecla 52:56399c2f13cd 25
cittecla 52:56399c2f13cd 26 int turn_for_deg(float deg)
cittecla 52:56399c2f13cd 27 {
cittecla 52:56399c2f13cd 28
cittecla 39:92723f7ea54f 29 return 0;
cittecla 52:56399c2f13cd 30 }
cittecla 52:56399c2f13cd 31
cittecla 52:56399c2f13cd 32
cittecla 52:56399c2f13cd 33 int move_to_next_coord()
cittecla 52:56399c2f13cd 34 {
cittecla 52:56399c2f13cd 35
cittecla 52:56399c2f13cd 36 float current_heading = get_current_heading();
cittecla 52:56399c2f13cd 37 position current_position = get_current_pos();
cittecla 52:56399c2f13cd 38 position next_pos = get_next_pos();
cittecla 39:92723f7ea54f 39
cittecla 52:56399c2f13cd 40 float needed_heading = 0;
cittecla 52:56399c2f13cd 41 float distance = 0;
cittecla 39:92723f7ea54f 42
cittecla 52:56399c2f13cd 43 // nord(-y) = 0 grad
cittecla 52:56399c2f13cd 44 if(current_pos.y > next_pos.y){
cittecla 52:56399c2f13cd 45 if(current_pos.x > next_pos.x) needed_heading = 315; distance = sqrt2;
cittecla 52:56399c2f13cd 46 if(current_pos.x = next_pos.x) needed_heading = 0; distance = 1;
cittecla 52:56399c2f13cd 47 if(current_pos.x < next_pos.x) needed_heading = 45; distance = sqrt2;
cittecla 52:56399c2f13cd 48 }
cittecla 52:56399c2f13cd 49 if(current_pos.y = next_pos.y){
cittecla 52:56399c2f13cd 50 if(current_pos.x > next_pos.x) needed_heading = 270; distance = 1;
cittecla 52:56399c2f13cd 51 if(current_pos.x = next_pos.x) //error same position;
cittecla 52:56399c2f13cd 52 if(current_pos.x < next_pos.x) needed_heading = 90; distance = 1;
cittecla 52:56399c2f13cd 53 }
cittecla 52:56399c2f13cd 54 if(current_pos.y < next_pos.y){
cittecla 52:56399c2f13cd 55 if(current_pos.x > next_pos.x) needed_heading = 225; distance = sqrt2;
cittecla 52:56399c2f13cd 56 if(current_pos.x = next_pos.x) needed_heading = 180; distance = 1;
cittecla 52:56399c2f13cd 57 if(current_pos.x < next_pos.x) needed_heading = 135; distance = sqrt2;
cittecla 52:56399c2f13cd 58 }
cittecla 52:56399c2f13cd 59
cittecla 52:56399c2f13cd 60 if(needed_heading != current_heading) {
cittecla 52:56399c2f13cd 61 turn_for_deg(needed_heading-current_heading);
cittecla 52:56399c2f13cd 62 } else {
cittecla 52:56399c2f13cd 63 move_forard_for_distance(distance);
cittecla 39:92723f7ea54f 64 }
cittecla 39:92723f7ea54f 65 return 0;
cittecla 52:56399c2f13cd 66 }
cittecla 52:56399c2f13cd 67
cittecla 52:56399c2f13cd 68 int move_in_search_for_brick()
cittecla 52:56399c2f13cd 69 {
cittecla 52:56399c2f13cd 70
cittecla 52:56399c2f13cd 71 return 0;
cittecla 52:56399c2f13cd 72 }
cittecla 52:56399c2f13cd 73
cittecla 39:92723f7ea54f 74
cittecla 39:92723f7ea54f 75
cittecla 39:92723f7ea54f 76
cittecla 39:92723f7ea54f 77
cittecla 39:92723f7ea54f 78
cittecla 39:92723f7ea54f 79
cittecla 39:92723f7ea54f 80
cittecla 39:92723f7ea54f 81
cittecla 39:92723f7ea54f 82
cittecla 39:92723f7ea54f 83
cittecla 39:92723f7ea54f 84
cittecla 39:92723f7ea54f 85
cittecla 39:92723f7ea54f 86
cittecla 39:92723f7ea54f 87
cittecla 39:92723f7ea54f 88
cittecla 39:92723f7ea54f 89
cittecla 39:92723f7ea54f 90
cittecla 39:92723f7ea54f 91
cittecla 39:92723f7ea54f 92
cittecla 39:92723f7ea54f 93
cittecla 31:51f52ffa4b51 94 /*
cittecla 18:a82994e67297 95
cittecla 18:a82994e67297 96 #include "mbed.h"
cittecla 27:df11ab63cda4 97 #include "movement.h"
cittecla 27:df11ab63cda4 98 #include "EncoderCounter.h"
cittecla 18:a82994e67297 99
cittecla 19:baa8371d55b4 100 static double time_counter = 0.0f;
cittecla 19:baa8371d55b4 101 static double timer0 = 0.0f;
cittecla 20:859c7aebf8a6 102 static float PID_correction_value = 1.0f;
cittecla 19:baa8371d55b4 103
cittecla 19:baa8371d55b4 104 static float power_value_slow = 0.6f;
cittecla 19:baa8371d55b4 105 static float power_value_medium = 0.7f;
cittecla 19:baa8371d55b4 106 static float power_value_fast = 0.8f;
cittecla 19:baa8371d55b4 107 static float ludicrous_value = 1.0f;
cittecla 19:baa8371d55b4 108
cittecla 27:df11ab63cda4 109 //Motor Encoders
cittecla 27:df11ab63cda4 110 EncoderCounter counterLeft(PB_6, PB_7);
cittecla 27:df11ab63cda4 111 EncoderCounter counterRight(PA_6, PC_7);
cittecla 20:859c7aebf8a6 112
cittecla 19:baa8371d55b4 113 //motor stuff
cittecla 19:baa8371d55b4 114 DigitalOut enableMotorDriver(PB_2);
cittecla 19:baa8371d55b4 115 PwmOut pwmL(PA_8);
cittecla 19:baa8371d55b4 116 PwmOut pwmR(PA_9);
cittecla 21:cb40c0533bc2 117 DigitalIn motorDriverFault(PB_14);
cittecla 21:cb40c0533bc2 118 DigitalIn motorDriverWarning(PB_15);
cittecla 21:cb40c0533bc2 119
cittecla 24:6c2fec64f890 120 //DigitalOut led(LED1); // Board LED
cittecla 21:cb40c0533bc2 121
cittecla 25:08ee4525155b 122 DigitalOut led(LED1); // Board LED
cittecla 18:a82994e67297 123
cittecla 26:58f90fa8dbaf 124
cittecla 31:51f52ffa4b51 125 // ******************************************************************************
cittecla 26:58f90fa8dbaf 126
cittecla 18:a82994e67297 127 void move_init()
cittecla 18:a82994e67297 128 {
cittecla 19:baa8371d55b4 129 pwmL.period(0.00005f); // Setzt die Periode auf 50 μs
cittecla 19:baa8371d55b4 130 pwmR.period(0.00005f);
cittecla 19:baa8371d55b4 131
cittecla 23:4ddc4216f335 132 pwmL.write(0.5f); // Setzt die Duty-Cycle auf 50%
cittecla 23:4ddc4216f335 133 pwmR.write(0.5f);
cittecla 21:cb40c0533bc2 134 enableMotorDriver = 1;
cittecla 21:cb40c0533bc2 135
cittecla 21:cb40c0533bc2 136 PID_correction_value = 1.0f;
cittecla 52:56399c2f13cd 137
cittecla 18:a82994e67297 138 }
cittecla 31:51f52ffa4b51 139 // ******************************************************************************
cittecla 20:859c7aebf8a6 140 void move_forward_slow(float correction_value)
cittecla 18:a82994e67297 141 {
cittecla 23:4ddc4216f335 142 pwmL.write(power_value_slow);
cittecla 29:e7d0208bf2af 143 pwmR.write(correction_value);
cittecla 29:e7d0208bf2af 144 printf("Left: %f || Right: %f value:%f \r\n",pwmL.read(), pwmR.read(), correction_value);
cittecla 29:e7d0208bf2af 145
cittecla 18:a82994e67297 146 }
cittecla 18:a82994e67297 147
cittecla 20:859c7aebf8a6 148 void move_forward_medium(float correction_value)
cittecla 18:a82994e67297 149 {
cittecla 19:baa8371d55b4 150 pwmL = power_value_medium;
cittecla 19:baa8371d55b4 151 pwmR = 1-power_value_medium*correction_value;
cittecla 18:a82994e67297 152 }
cittecla 18:a82994e67297 153
cittecla 20:859c7aebf8a6 154 void move_backward_slow(float correction_value)
cittecla 18:a82994e67297 155 {
cittecla 19:baa8371d55b4 156 pwmL = 1-power_value_slow*correction_value;
cittecla 19:baa8371d55b4 157 pwmR = power_value_slow;
cittecla 18:a82994e67297 158 }
cittecla 18:a82994e67297 159
cittecla 20:859c7aebf8a6 160 void move_backward_medium(float correction_value)
cittecla 18:a82994e67297 161 {
cittecla 19:baa8371d55b4 162 pwmL = 1-power_value_slow*correction_value;
cittecla 19:baa8371d55b4 163 pwmR = power_value_slow;
cittecla 22:c8e187b9d949 164
cittecla 18:a82994e67297 165 }
cittecla 31:51f52ffa4b51 166 // ******************************************************************************
cittecla 18:a82994e67297 167
cittecla 19:baa8371d55b4 168 void stop_movement()
cittecla 19:baa8371d55b4 169 {
cittecla 21:cb40c0533bc2 170 pwmL = 0.5f;
cittecla 21:cb40c0533bc2 171 pwmR = 0.5f;
cittecla 27:df11ab63cda4 172 counterLeft.reset();
cittecla 27:df11ab63cda4 173 counterRight.reset();
cittecla 18:a82994e67297 174 }
cittecla 18:a82994e67297 175
cittecla 21:cb40c0533bc2 176 void sync_movement(bool speed, bool direction)
cittecla 21:cb40c0533bc2 177 {
cittecla 29:e7d0208bf2af 178 if(counterLeft.read() > 30000 || -counterRight > 30000){
cittecla 52:56399c2f13cd 179
cittecla 29:e7d0208bf2af 180 }
cittecla 27:df11ab63cda4 181 printf("Left: %d || Right: %d\r\n",counterLeft.read(), -counterRight.read());
cittecla 27:df11ab63cda4 182 if(counterLeft.read() > -counterRight.read()) {
cittecla 29:e7d0208bf2af 183 PID_correction_value += 0.001f;
cittecla 23:4ddc4216f335 184 } else {
cittecla 27:df11ab63cda4 185 if(counterLeft.read() < -counterRight.read()) {
cittecla 29:e7d0208bf2af 186 PID_correction_value -= 0.001f;
cittecla 23:4ddc4216f335 187 } else {
cittecla 52:56399c2f13cd 188 // even
cittecla 23:4ddc4216f335 189 }
cittecla 23:4ddc4216f335 190 }
cittecla 22:c8e187b9d949 191
cittecla 25:08ee4525155b 192 if(PID_correction_value < 0.0f) {
cittecla 25:08ee4525155b 193 PID_correction_value = 0;
cittecla 25:08ee4525155b 194 }
cittecla 25:08ee4525155b 195 if(PID_correction_value > 2.0f) {
cittecla 25:08ee4525155b 196 PID_correction_value = 2;
cittecla 25:08ee4525155b 197 }
cittecla 22:c8e187b9d949 198
cittecla 20:859c7aebf8a6 199 // Call movement:
cittecla 20:859c7aebf8a6 200 // direction 0 = backward, direction 1 = forward
cittecla 20:859c7aebf8a6 201 // speed 0 = slow, speed 1 = medium
cittecla 21:cb40c0533bc2 202
cittecla 31:51f52ffa4b51 203 if(direction && speed) {
cittecla 20:859c7aebf8a6 204 move_forward_medium(PID_correction_value);
cittecla 31:51f52ffa4b51 205 }
cittecla 20:859c7aebf8a6 206 if(direction && !speed) {
cittecla 29:e7d0208bf2af 207 float value = 1.0f-power_value_slow*PID_correction_value;
cittecla 29:e7d0208bf2af 208 move_forward_slow(value);
cittecla 31:51f52ffa4b51 209 }
cittecla 20:859c7aebf8a6 210 if(!direction && speed) {
cittecla 20:859c7aebf8a6 211 move_backward_medium(PID_correction_value);
cittecla 20:859c7aebf8a6 212 }
cittecla 21:cb40c0533bc2 213 if(!direction && !speed) {
cittecla 22:c8e187b9d949 214 move_backward_slow(PID_correction_value);
cittecla 31:51f52ffa4b51 215 }
cittecla 20:859c7aebf8a6 216 }
cittecla 20:859c7aebf8a6 217
cittecla 26:58f90fa8dbaf 218 void terminate_movement()
cittecla 21:cb40c0533bc2 219 {
cittecla 20:859c7aebf8a6 220 PID_correction_value = 1.0f;
cittecla 26:58f90fa8dbaf 221 pwmL.write(0.5f);
cittecla 26:58f90fa8dbaf 222 pwmR.write(0.5f);
cittecla 21:cb40c0533bc2 223 }
cittecla 31:51f52ffa4b51 224 */