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@25:08ee4525155b, 2017-03-05 (annotated)

Committer:
cittecla
Date:
Sun Mar 05 12:18:04 2017 +0000
Revision:
25:08ee4525155b
Parent:
24:6c2fec64f890
Child:
26:58f90fa8dbaf
test

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 18:a82994e67297 5
cittecla 18:a82994e67297 6
cittecla 18:a82994e67297 7 #include "mbed.h"
cittecla 19:baa8371d55b4 8 #include "move.h"
cittecla 18:a82994e67297 9
cittecla 19:baa8371d55b4 10 static double time_counter = 0.0f;
cittecla 19:baa8371d55b4 11 static double timer0 = 0.0f;
cittecla 20:859c7aebf8a6 12 static float PID_correction_value = 1.0f;
cittecla 19:baa8371d55b4 13
cittecla 19:baa8371d55b4 14 static float power_value_slow = 0.6f;
cittecla 19:baa8371d55b4 15 static float power_value_medium = 0.7f;
cittecla 19:baa8371d55b4 16 static float power_value_fast = 0.8f;
cittecla 19:baa8371d55b4 17 static float ludicrous_value = 1.0f;
cittecla 19:baa8371d55b4 18
cittecla 20:859c7aebf8a6 19
cittecla 19:baa8371d55b4 20 //motor stuff
cittecla 19:baa8371d55b4 21 DigitalOut enableMotorDriver(PB_2);
cittecla 19:baa8371d55b4 22 PwmOut pwmL(PA_8);
cittecla 19:baa8371d55b4 23 PwmOut pwmR(PA_9);
cittecla 21:cb40c0533bc2 24 DigitalIn motorDriverFault(PB_14);
cittecla 21:cb40c0533bc2 25 DigitalIn motorDriverWarning(PB_15);
cittecla 21:cb40c0533bc2 26
cittecla 24:6c2fec64f890 27 //DigitalOut led(LED1); // Board LED
cittecla 21:cb40c0533bc2 28
cittecla 21:cb40c0533bc2 29 static int EncoderCounterLeft = 0;
cittecla 21:cb40c0533bc2 30 static int EncoderCounterRight = 0;
cittecla 25:08ee4525155b 31 DigitalOut led(LED1); // Board LED
cittecla 18:a82994e67297 32
cittecla 18:a82994e67297 33 void move_init()
cittecla 18:a82994e67297 34 {
cittecla 19:baa8371d55b4 35 pwmL.period(0.00005f); // Setzt die Periode auf 50 μs
cittecla 19:baa8371d55b4 36 pwmR.period(0.00005f);
cittecla 19:baa8371d55b4 37
cittecla 23:4ddc4216f335 38 pwmL.write(0.5f); // Setzt die Duty-Cycle auf 50%
cittecla 23:4ddc4216f335 39 pwmR.write(0.5f);
cittecla 21:cb40c0533bc2 40 enableMotorDriver = 1;
cittecla 21:cb40c0533bc2 41
cittecla 21:cb40c0533bc2 42 EncoderCounterLeft = 0;
cittecla 21:cb40c0533bc2 43 EncoderCounterRight = 0;
cittecla 21:cb40c0533bc2 44 PID_correction_value = 1.0f;
cittecla 18:a82994e67297 45 }
cittecla 18:a82994e67297 46
cittecla 20:859c7aebf8a6 47 void move_forward_slow(float correction_value)
cittecla 18:a82994e67297 48 {
cittecla 23:4ddc4216f335 49 pwmL.write(power_value_slow);
cittecla 23:4ddc4216f335 50 pwmR.write(1-power_value_slow*correction_value);
cittecla 22:c8e187b9d949 51
cittecla 18:a82994e67297 52 }
cittecla 18:a82994e67297 53
cittecla 20:859c7aebf8a6 54 void move_forward_medium(float correction_value)
cittecla 18:a82994e67297 55 {
cittecla 19:baa8371d55b4 56 pwmL = power_value_medium;
cittecla 19:baa8371d55b4 57 pwmR = 1-power_value_medium*correction_value;
cittecla 18:a82994e67297 58 }
cittecla 18:a82994e67297 59
cittecla 20:859c7aebf8a6 60 void move_forward_fast(float correction_value)
cittecla 18:a82994e67297 61 {
cittecla 19:baa8371d55b4 62 pwmL = power_value_fast;
cittecla 19:baa8371d55b4 63 pwmR = 1-power_value_fast*correction_value;
cittecla 18:a82994e67297 64 }
cittecla 18:a82994e67297 65
cittecla 20:859c7aebf8a6 66 void move_backward_slow(float correction_value)
cittecla 18:a82994e67297 67 {
cittecla 19:baa8371d55b4 68 pwmL = 1-power_value_slow*correction_value;
cittecla 19:baa8371d55b4 69 pwmR = power_value_slow;
cittecla 18:a82994e67297 70 }
cittecla 18:a82994e67297 71
cittecla 20:859c7aebf8a6 72 void move_backward_medium(float correction_value)
cittecla 18:a82994e67297 73 {
cittecla 19:baa8371d55b4 74 pwmL = 1-power_value_slow*correction_value;
cittecla 19:baa8371d55b4 75 pwmR = power_value_slow;
cittecla 22:c8e187b9d949 76
cittecla 18:a82994e67297 77 }
cittecla 18:a82994e67297 78
cittecla 20:859c7aebf8a6 79 void move_backward_fast(float correction_value)
cittecla 18:a82994e67297 80 {
cittecla 19:baa8371d55b4 81 pwmL = 1-power_value_slow*correction_value;
cittecla 19:baa8371d55b4 82 pwmR = power_value_slow;
cittecla 19:baa8371d55b4 83 }
cittecla 18:a82994e67297 84
cittecla 19:baa8371d55b4 85 void stop_movement()
cittecla 19:baa8371d55b4 86 {
cittecla 21:cb40c0533bc2 87 pwmL = 0.5f;
cittecla 21:cb40c0533bc2 88 pwmR = 0.5f;
cittecla 18:a82994e67297 89 }
cittecla 18:a82994e67297 90
cittecla 25:08ee4525155b 91 // Encoder Interrupt
cittecla 21:cb40c0533bc2 92 void highPulseDetectedL()
cittecla 21:cb40c0533bc2 93 {
cittecla 21:cb40c0533bc2 94 EncoderCounterLeft += 1;
cittecla 21:cb40c0533bc2 95 }
cittecla 21:cb40c0533bc2 96
cittecla 21:cb40c0533bc2 97 void highPulseDetectedR()
cittecla 21:cb40c0533bc2 98 {
cittecla 25:08ee4525155b 99 EncoderCounterRight += 1;
cittecla 21:cb40c0533bc2 100 }
cittecla 18:a82994e67297 101
cittecla 21:cb40c0533bc2 102 void sync_movement(bool speed, bool direction)
cittecla 21:cb40c0533bc2 103 {
cittecla 21:cb40c0533bc2 104
cittecla 21:cb40c0533bc2 105 // PID correction Value calcualtion
cittecla 25:08ee4525155b 106 //printf("left: %d || right: %d\r\n",EncoderCounterLeft,EncoderCounterRight);
cittecla 23:4ddc4216f335 107 if(EncoderCounterLeft > EncoderCounterRight) {
cittecla 23:4ddc4216f335 108 PID_correction_value += 0.0001f;
cittecla 22:c8e187b9d949 109
cittecla 25:08ee4525155b 110 // printf("Left higher ");
cittecla 23:4ddc4216f335 111 } else {
cittecla 23:4ddc4216f335 112 if(EncoderCounterLeft < EncoderCounterRight) {
cittecla 23:4ddc4216f335 113 PID_correction_value -= 0.0001f;
cittecla 25:08ee4525155b 114 // printf("Right higher ");
cittecla 23:4ddc4216f335 115 } else {
cittecla 25:08ee4525155b 116 // printf("Even ");
cittecla 23:4ddc4216f335 117 }
cittecla 23:4ddc4216f335 118 }
cittecla 22:c8e187b9d949 119
cittecla 25:08ee4525155b 120 if(PID_correction_value < 0.0f) {
cittecla 25:08ee4525155b 121 PID_correction_value = 0;
cittecla 25:08ee4525155b 122 }
cittecla 25:08ee4525155b 123 if(PID_correction_value > 2.0f) {
cittecla 25:08ee4525155b 124 PID_correction_value = 2;
cittecla 25:08ee4525155b 125 }
cittecla 22:c8e187b9d949 126
cittecla 20:859c7aebf8a6 127 // Call movement:
cittecla 20:859c7aebf8a6 128 // direction 0 = backward, direction 1 = forward
cittecla 20:859c7aebf8a6 129 // speed 0 = slow, speed 1 = medium
cittecla 21:cb40c0533bc2 130
cittecla 20:859c7aebf8a6 131 if(direction && speed) {
cittecla 20:859c7aebf8a6 132 move_forward_medium(PID_correction_value);
cittecla 20:859c7aebf8a6 133 }
cittecla 20:859c7aebf8a6 134 if(direction && !speed) {
cittecla 20:859c7aebf8a6 135 move_forward_slow(PID_correction_value);
cittecla 20:859c7aebf8a6 136 }
cittecla 20:859c7aebf8a6 137 if(!direction && speed) {
cittecla 20:859c7aebf8a6 138 move_backward_medium(PID_correction_value);
cittecla 20:859c7aebf8a6 139 }
cittecla 21:cb40c0533bc2 140 if(!direction && !speed) {
cittecla 22:c8e187b9d949 141 move_backward_slow(PID_correction_value);
cittecla 20:859c7aebf8a6 142 }
cittecla 20:859c7aebf8a6 143 }
cittecla 20:859c7aebf8a6 144
cittecla 21:cb40c0533bc2 145 void termiante_sync_movement()
cittecla 21:cb40c0533bc2 146 {
cittecla 20:859c7aebf8a6 147 PID_correction_value = 1.0f;
cittecla 21:cb40c0533bc2 148 }
cittecla 20:859c7aebf8a6 149