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@26:58f90fa8dbaf, 2017-03-05 (annotated)

Committer:
cittecla
Date:
Sun Mar 05 17:11:17 2017 +0000
Revision:
26:58f90fa8dbaf
Parent:
25:08ee4525155b
Child:
27:df11ab63cda4
..

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 26:58f90fa8dbaf 33 // Encoder Interrupt ***********************************************************
cittecla 26:58f90fa8dbaf 34 void highPulseDetectedL()
cittecla 26:58f90fa8dbaf 35 {
cittecla 26:58f90fa8dbaf 36 EncoderCounterLeft += 1;
cittecla 26:58f90fa8dbaf 37 }
cittecla 26:58f90fa8dbaf 38
cittecla 26:58f90fa8dbaf 39 void highPulseDetectedR()
cittecla 26:58f90fa8dbaf 40 {
cittecla 26:58f90fa8dbaf 41 EncoderCounterRight += 1;
cittecla 26:58f90fa8dbaf 42 }
cittecla 26:58f90fa8dbaf 43
cittecla 26:58f90fa8dbaf 44 //******************************************************************************
cittecla 26:58f90fa8dbaf 45
cittecla 18:a82994e67297 46 void move_init()
cittecla 18:a82994e67297 47 {
cittecla 19:baa8371d55b4 48 pwmL.period(0.00005f); // Setzt die Periode auf 50 μs
cittecla 19:baa8371d55b4 49 pwmR.period(0.00005f);
cittecla 19:baa8371d55b4 50
cittecla 23:4ddc4216f335 51 pwmL.write(0.5f); // Setzt die Duty-Cycle auf 50%
cittecla 23:4ddc4216f335 52 pwmR.write(0.5f);
cittecla 21:cb40c0533bc2 53 enableMotorDriver = 1;
cittecla 21:cb40c0533bc2 54
cittecla 21:cb40c0533bc2 55 EncoderCounterLeft = 0;
cittecla 21:cb40c0533bc2 56 EncoderCounterRight = 0;
cittecla 21:cb40c0533bc2 57 PID_correction_value = 1.0f;
cittecla 18:a82994e67297 58 }
cittecla 26:58f90fa8dbaf 59 //******************************************************************************
cittecla 20:859c7aebf8a6 60 void move_forward_slow(float correction_value)
cittecla 18:a82994e67297 61 {
cittecla 23:4ddc4216f335 62 pwmL.write(power_value_slow);
cittecla 23:4ddc4216f335 63 pwmR.write(1-power_value_slow*correction_value);
cittecla 18:a82994e67297 64 }
cittecla 18:a82994e67297 65
cittecla 20:859c7aebf8a6 66 void move_forward_medium(float correction_value)
cittecla 18:a82994e67297 67 {
cittecla 19:baa8371d55b4 68 pwmL = power_value_medium;
cittecla 19:baa8371d55b4 69 pwmR = 1-power_value_medium*correction_value;
cittecla 18:a82994e67297 70 }
cittecla 18:a82994e67297 71
cittecla 20:859c7aebf8a6 72 void move_backward_slow(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 18:a82994e67297 76 }
cittecla 18:a82994e67297 77
cittecla 20:859c7aebf8a6 78 void move_backward_medium(float correction_value)
cittecla 18:a82994e67297 79 {
cittecla 19:baa8371d55b4 80 pwmL = 1-power_value_slow*correction_value;
cittecla 19:baa8371d55b4 81 pwmR = power_value_slow;
cittecla 22:c8e187b9d949 82
cittecla 18:a82994e67297 83 }
cittecla 26:58f90fa8dbaf 84 //******************************************************************************
cittecla 18:a82994e67297 85
cittecla 19:baa8371d55b4 86 void stop_movement()
cittecla 19:baa8371d55b4 87 {
cittecla 21:cb40c0533bc2 88 pwmL = 0.5f;
cittecla 21:cb40c0533bc2 89 pwmR = 0.5f;
cittecla 18:a82994e67297 90 }
cittecla 18:a82994e67297 91
cittecla 21:cb40c0533bc2 92 void sync_movement(bool speed, bool direction)
cittecla 21:cb40c0533bc2 93 {
cittecla 23:4ddc4216f335 94 if(EncoderCounterLeft > EncoderCounterRight) {
cittecla 23:4ddc4216f335 95 PID_correction_value += 0.0001f;
cittecla 23:4ddc4216f335 96 } else {
cittecla 23:4ddc4216f335 97 if(EncoderCounterLeft < EncoderCounterRight) {
cittecla 23:4ddc4216f335 98 PID_correction_value -= 0.0001f;
cittecla 23:4ddc4216f335 99 } else {
cittecla 26:58f90fa8dbaf 100 // even
cittecla 23:4ddc4216f335 101 }
cittecla 23:4ddc4216f335 102 }
cittecla 22:c8e187b9d949 103
cittecla 25:08ee4525155b 104 if(PID_correction_value < 0.0f) {
cittecla 25:08ee4525155b 105 PID_correction_value = 0;
cittecla 25:08ee4525155b 106 }
cittecla 25:08ee4525155b 107 if(PID_correction_value > 2.0f) {
cittecla 25:08ee4525155b 108 PID_correction_value = 2;
cittecla 25:08ee4525155b 109 }
cittecla 22:c8e187b9d949 110
cittecla 20:859c7aebf8a6 111 // Call movement:
cittecla 20:859c7aebf8a6 112 // direction 0 = backward, direction 1 = forward
cittecla 20:859c7aebf8a6 113 // speed 0 = slow, speed 1 = medium
cittecla 21:cb40c0533bc2 114
cittecla 20:859c7aebf8a6 115 if(direction && speed) {
cittecla 20:859c7aebf8a6 116 move_forward_medium(PID_correction_value);
cittecla 20:859c7aebf8a6 117 }
cittecla 20:859c7aebf8a6 118 if(direction && !speed) {
cittecla 20:859c7aebf8a6 119 move_forward_slow(PID_correction_value);
cittecla 20:859c7aebf8a6 120 }
cittecla 20:859c7aebf8a6 121 if(!direction && speed) {
cittecla 20:859c7aebf8a6 122 move_backward_medium(PID_correction_value);
cittecla 20:859c7aebf8a6 123 }
cittecla 21:cb40c0533bc2 124 if(!direction && !speed) {
cittecla 22:c8e187b9d949 125 move_backward_slow(PID_correction_value);
cittecla 20:859c7aebf8a6 126 }
cittecla 20:859c7aebf8a6 127 }
cittecla 20:859c7aebf8a6 128
cittecla 26:58f90fa8dbaf 129 void terminate_movement()
cittecla 21:cb40c0533bc2 130 {
cittecla 20:859c7aebf8a6 131 PID_correction_value = 1.0f;
cittecla 26:58f90fa8dbaf 132 pwmL.write(0.5f);
cittecla 26:58f90fa8dbaf 133 pwmR.write(0.5f);
cittecla 21:cb40c0533bc2 134 }
cittecla 20:859c7aebf8a6 135