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@27:df11ab63cda4, 2017-03-06 (annotated)

Committer:
cittecla
Date:
Mon Mar 06 15:10:24 2017 +0000
Revision:
27:df11ab63cda4
Parent:
26:58f90fa8dbaf
Child:
29:e7d0208bf2af
Implemented encoder counter

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