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@44:7118b23b0fd7, 2017-04-11 (annotated)

Committer:
cittecla
Date:
Tue Apr 11 14:13:37 2017 +0000
Revision:
44:7118b23b0fd7
Parent:
39:92723f7ea54f 
Nothing

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 39:92723f7ea54f 8 int moving(){
cittecla 39:92723f7ea54f 9
cittecla 39:92723f7ea54f 10 return 0;
cittecla 39:92723f7ea54f 11 }
cittecla 39:92723f7ea54f 12
cittecla 39:92723f7ea54f 13 int moving_forward_for_distance(){
cittecla 39:92723f7ea54f 14
cittecla 39:92723f7ea54f 15 return 0;
cittecla 39:92723f7ea54f 16 }
cittecla 39:92723f7ea54f 17
cittecla 39:92723f7ea54f 18 int moving_backward_for_distance(){
cittecla 39:92723f7ea54f 19
cittecla 39:92723f7ea54f 20 return 0;
cittecla 39:92723f7ea54f 21 }
cittecla 39:92723f7ea54f 22
cittecla 39:92723f7ea54f 23 int turn_left_for_deg(){
cittecla 39:92723f7ea54f 24
cittecla 39:92723f7ea54f 25 return 0;
cittecla 39:92723f7ea54f 26 }
cittecla 39:92723f7ea54f 27
cittecla 39:92723f7ea54f 28 int turn_right_for_deg(){
cittecla 39:92723f7ea54f 29
cittecla 39:92723f7ea54f 30 return 0;
cittecla 39:92723f7ea54f 31 }
cittecla 39:92723f7ea54f 32
cittecla 39:92723f7ea54f 33 int move_to_next_coord(){
cittecla 39:92723f7ea54f 34
cittecla 39:92723f7ea54f 35 return 0;
cittecla 39:92723f7ea54f 36 }
cittecla 39:92723f7ea54f 37
cittecla 39:92723f7ea54f 38 int move_in_search_for_brick(){
cittecla 39:92723f7ea54f 39
cittecla 39:92723f7ea54f 40 return 0;
cittecla 39:92723f7ea54f 41 }
cittecla 39:92723f7ea54f 42
cittecla 39:92723f7ea54f 43
cittecla 39:92723f7ea54f 44
cittecla 39:92723f7ea54f 45
cittecla 39:92723f7ea54f 46
cittecla 39:92723f7ea54f 47
cittecla 31:51f52ffa4b51 48 /*
cittecla 18:a82994e67297 49
cittecla 18:a82994e67297 50 #include "mbed.h"
cittecla 27:df11ab63cda4 51 #include "movement.h"
cittecla 27:df11ab63cda4 52 #include "EncoderCounter.h"
cittecla 18:a82994e67297 53
cittecla 19:baa8371d55b4 54 static double time_counter = 0.0f;
cittecla 19:baa8371d55b4 55 static double timer0 = 0.0f;
cittecla 20:859c7aebf8a6 56 static float PID_correction_value = 1.0f;
cittecla 19:baa8371d55b4 57
cittecla 19:baa8371d55b4 58 static float power_value_slow = 0.6f;
cittecla 19:baa8371d55b4 59 static float power_value_medium = 0.7f;
cittecla 19:baa8371d55b4 60 static float power_value_fast = 0.8f;
cittecla 19:baa8371d55b4 61 static float ludicrous_value = 1.0f;
cittecla 19:baa8371d55b4 62
cittecla 27:df11ab63cda4 63 //Motor Encoders
cittecla 27:df11ab63cda4 64 EncoderCounter counterLeft(PB_6, PB_7);
cittecla 27:df11ab63cda4 65 EncoderCounter counterRight(PA_6, PC_7);
cittecla 20:859c7aebf8a6 66
cittecla 19:baa8371d55b4 67 //motor stuff
cittecla 19:baa8371d55b4 68 DigitalOut enableMotorDriver(PB_2);
cittecla 19:baa8371d55b4 69 PwmOut pwmL(PA_8);
cittecla 19:baa8371d55b4 70 PwmOut pwmR(PA_9);
cittecla 21:cb40c0533bc2 71 DigitalIn motorDriverFault(PB_14);
cittecla 21:cb40c0533bc2 72 DigitalIn motorDriverWarning(PB_15);
cittecla 21:cb40c0533bc2 73
cittecla 24:6c2fec64f890 74 //DigitalOut led(LED1); // Board LED
cittecla 21:cb40c0533bc2 75
cittecla 25:08ee4525155b 76 DigitalOut led(LED1); // Board LED
cittecla 18:a82994e67297 77
cittecla 26:58f90fa8dbaf 78
cittecla 31:51f52ffa4b51 79 // ******************************************************************************
cittecla 26:58f90fa8dbaf 80
cittecla 18:a82994e67297 81 void move_init()
cittecla 18:a82994e67297 82 {
cittecla 19:baa8371d55b4 83 pwmL.period(0.00005f); // Setzt die Periode auf 50 μs
cittecla 19:baa8371d55b4 84 pwmR.period(0.00005f);
cittecla 19:baa8371d55b4 85
cittecla 23:4ddc4216f335 86 pwmL.write(0.5f); // Setzt die Duty-Cycle auf 50%
cittecla 23:4ddc4216f335 87 pwmR.write(0.5f);
cittecla 21:cb40c0533bc2 88 enableMotorDriver = 1;
cittecla 21:cb40c0533bc2 89
cittecla 21:cb40c0533bc2 90 PID_correction_value = 1.0f;
cittecla 27:df11ab63cda4 91
cittecla 18:a82994e67297 92 }
cittecla 31:51f52ffa4b51 93 // ******************************************************************************
cittecla 20:859c7aebf8a6 94 void move_forward_slow(float correction_value)
cittecla 18:a82994e67297 95 {
cittecla 23:4ddc4216f335 96 pwmL.write(power_value_slow);
cittecla 29:e7d0208bf2af 97 pwmR.write(correction_value);
cittecla 29:e7d0208bf2af 98 printf("Left: %f || Right: %f value:%f \r\n",pwmL.read(), pwmR.read(), correction_value);
cittecla 29:e7d0208bf2af 99
cittecla 18:a82994e67297 100 }
cittecla 18:a82994e67297 101
cittecla 20:859c7aebf8a6 102 void move_forward_medium(float correction_value)
cittecla 18:a82994e67297 103 {
cittecla 19:baa8371d55b4 104 pwmL = power_value_medium;
cittecla 19:baa8371d55b4 105 pwmR = 1-power_value_medium*correction_value;
cittecla 18:a82994e67297 106 }
cittecla 18:a82994e67297 107
cittecla 20:859c7aebf8a6 108 void move_backward_slow(float correction_value)
cittecla 18:a82994e67297 109 {
cittecla 19:baa8371d55b4 110 pwmL = 1-power_value_slow*correction_value;
cittecla 19:baa8371d55b4 111 pwmR = power_value_slow;
cittecla 18:a82994e67297 112 }
cittecla 18:a82994e67297 113
cittecla 20:859c7aebf8a6 114 void move_backward_medium(float correction_value)
cittecla 18:a82994e67297 115 {
cittecla 19:baa8371d55b4 116 pwmL = 1-power_value_slow*correction_value;
cittecla 19:baa8371d55b4 117 pwmR = power_value_slow;
cittecla 22:c8e187b9d949 118
cittecla 18:a82994e67297 119 }
cittecla 31:51f52ffa4b51 120 // ******************************************************************************
cittecla 18:a82994e67297 121
cittecla 19:baa8371d55b4 122 void stop_movement()
cittecla 19:baa8371d55b4 123 {
cittecla 21:cb40c0533bc2 124 pwmL = 0.5f;
cittecla 21:cb40c0533bc2 125 pwmR = 0.5f;
cittecla 27:df11ab63cda4 126 counterLeft.reset();
cittecla 27:df11ab63cda4 127 counterRight.reset();
cittecla 18:a82994e67297 128 }
cittecla 18:a82994e67297 129
cittecla 21:cb40c0533bc2 130 void sync_movement(bool speed, bool direction)
cittecla 21:cb40c0533bc2 131 {
cittecla 29:e7d0208bf2af 132 if(counterLeft.read() > 30000 || -counterRight > 30000){
cittecla 29:e7d0208bf2af 133
cittecla 29:e7d0208bf2af 134 }
cittecla 27:df11ab63cda4 135 printf("Left: %d || Right: %d\r\n",counterLeft.read(), -counterRight.read());
cittecla 27:df11ab63cda4 136 if(counterLeft.read() > -counterRight.read()) {
cittecla 29:e7d0208bf2af 137 PID_correction_value += 0.001f;
cittecla 23:4ddc4216f335 138 } else {
cittecla 27:df11ab63cda4 139 if(counterLeft.read() < -counterRight.read()) {
cittecla 29:e7d0208bf2af 140 PID_correction_value -= 0.001f;
cittecla 23:4ddc4216f335 141 } else {
cittecla 26:58f90fa8dbaf 142 // even
cittecla 23:4ddc4216f335 143 }
cittecla 23:4ddc4216f335 144 }
cittecla 22:c8e187b9d949 145
cittecla 25:08ee4525155b 146 if(PID_correction_value < 0.0f) {
cittecla 25:08ee4525155b 147 PID_correction_value = 0;
cittecla 25:08ee4525155b 148 }
cittecla 25:08ee4525155b 149 if(PID_correction_value > 2.0f) {
cittecla 25:08ee4525155b 150 PID_correction_value = 2;
cittecla 25:08ee4525155b 151 }
cittecla 22:c8e187b9d949 152
cittecla 20:859c7aebf8a6 153 // Call movement:
cittecla 20:859c7aebf8a6 154 // direction 0 = backward, direction 1 = forward
cittecla 20:859c7aebf8a6 155 // speed 0 = slow, speed 1 = medium
cittecla 21:cb40c0533bc2 156
cittecla 31:51f52ffa4b51 157 if(direction && speed) {
cittecla 20:859c7aebf8a6 158 move_forward_medium(PID_correction_value);
cittecla 31:51f52ffa4b51 159 }
cittecla 20:859c7aebf8a6 160 if(direction && !speed) {
cittecla 29:e7d0208bf2af 161 float value = 1.0f-power_value_slow*PID_correction_value;
cittecla 29:e7d0208bf2af 162 move_forward_slow(value);
cittecla 31:51f52ffa4b51 163 }
cittecla 20:859c7aebf8a6 164 if(!direction && speed) {
cittecla 20:859c7aebf8a6 165 move_backward_medium(PID_correction_value);
cittecla 20:859c7aebf8a6 166 }
cittecla 21:cb40c0533bc2 167 if(!direction && !speed) {
cittecla 22:c8e187b9d949 168 move_backward_slow(PID_correction_value);
cittecla 31:51f52ffa4b51 169 }
cittecla 20:859c7aebf8a6 170 }
cittecla 20:859c7aebf8a6 171
cittecla 26:58f90fa8dbaf 172 void terminate_movement()
cittecla 21:cb40c0533bc2 173 {
cittecla 20:859c7aebf8a6 174 PID_correction_value = 1.0f;
cittecla 26:58f90fa8dbaf 175 pwmL.write(0.5f);
cittecla 26:58f90fa8dbaf 176 pwmR.write(0.5f);
cittecla 21:cb40c0533bc2 177 }
cittecla 31:51f52ffa4b51 178 */