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@39:92723f7ea54f, 2017-04-04 (annotated)

Committer:
cittecla
Date:
Tue Apr 04 15:28:26 2017 +0000
Revision:
39:92723f7ea54f
Parent:
34:40d8d29b44b8
Child:
44:7118b23b0fd7
Child:
52:56399c2f13cd
Default function implementation complete.; State Machine working properly.; Compiled succesfully

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