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@31:51f52ffa4b51, 2017-03-14 (annotated)

Committer:
cittecla
Date:
Tue Mar 14 16:24:15 2017 +0000
Revision:
31:51f52ffa4b51
Parent:
29:e7d0208bf2af 
something

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 31:51f52ffa4b51 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 31:51f52ffa4b51 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 31:51f52ffa4b51 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 29:e7d0208bf2af 54 pwmR.write(correction_value);
cittecla 29:e7d0208bf2af 55 printf("Left: %f || Right: %f value:%f \r\n",pwmL.read(), pwmR.read(), correction_value);
cittecla 29:e7d0208bf2af 56
cittecla 18:a82994e67297 57 }
cittecla 18:a82994e67297 58
cittecla 20:859c7aebf8a6 59 void move_forward_medium(float correction_value)
cittecla 18:a82994e67297 60 {
cittecla 19:baa8371d55b4 61 pwmL = power_value_medium;
cittecla 19:baa8371d55b4 62 pwmR = 1-power_value_medium*correction_value;
cittecla 18:a82994e67297 63 }
cittecla 18:a82994e67297 64
cittecla 20:859c7aebf8a6 65 void move_backward_slow(float correction_value)
cittecla 18:a82994e67297 66 {
cittecla 19:baa8371d55b4 67 pwmL = 1-power_value_slow*correction_value;
cittecla 19:baa8371d55b4 68 pwmR = power_value_slow;
cittecla 18:a82994e67297 69 }
cittecla 18:a82994e67297 70
cittecla 20:859c7aebf8a6 71 void move_backward_medium(float correction_value)
cittecla 18:a82994e67297 72 {
cittecla 19:baa8371d55b4 73 pwmL = 1-power_value_slow*correction_value;
cittecla 19:baa8371d55b4 74 pwmR = power_value_slow;
cittecla 22:c8e187b9d949 75
cittecla 18:a82994e67297 76 }
cittecla 31:51f52ffa4b51 77 // ******************************************************************************
cittecla 18:a82994e67297 78
cittecla 19:baa8371d55b4 79 void stop_movement()
cittecla 19:baa8371d55b4 80 {
cittecla 21:cb40c0533bc2 81 pwmL = 0.5f;
cittecla 21:cb40c0533bc2 82 pwmR = 0.5f;
cittecla 27:df11ab63cda4 83 counterLeft.reset();
cittecla 27:df11ab63cda4 84 counterRight.reset();
cittecla 18:a82994e67297 85 }
cittecla 18:a82994e67297 86
cittecla 21:cb40c0533bc2 87 void sync_movement(bool speed, bool direction)
cittecla 21:cb40c0533bc2 88 {
cittecla 29:e7d0208bf2af 89 if(counterLeft.read() > 30000 || -counterRight > 30000){
cittecla 29:e7d0208bf2af 90
cittecla 29:e7d0208bf2af 91 }
cittecla 27:df11ab63cda4 92 printf("Left: %d || Right: %d\r\n",counterLeft.read(), -counterRight.read());
cittecla 27:df11ab63cda4 93 if(counterLeft.read() > -counterRight.read()) {
cittecla 29:e7d0208bf2af 94 PID_correction_value += 0.001f;
cittecla 23:4ddc4216f335 95 } else {
cittecla 27:df11ab63cda4 96 if(counterLeft.read() < -counterRight.read()) {
cittecla 29:e7d0208bf2af 97 PID_correction_value -= 0.001f;
cittecla 23:4ddc4216f335 98 } else {
cittecla 26:58f90fa8dbaf 99 // even
cittecla 23:4ddc4216f335 100 }
cittecla 23:4ddc4216f335 101 }
cittecla 22:c8e187b9d949 102
cittecla 25:08ee4525155b 103 if(PID_correction_value < 0.0f) {
cittecla 25:08ee4525155b 104 PID_correction_value = 0;
cittecla 25:08ee4525155b 105 }
cittecla 25:08ee4525155b 106 if(PID_correction_value > 2.0f) {
cittecla 25:08ee4525155b 107 PID_correction_value = 2;
cittecla 25:08ee4525155b 108 }
cittecla 22:c8e187b9d949 109
cittecla 20:859c7aebf8a6 110 // Call movement:
cittecla 20:859c7aebf8a6 111 // direction 0 = backward, direction 1 = forward
cittecla 20:859c7aebf8a6 112 // speed 0 = slow, speed 1 = medium
cittecla 21:cb40c0533bc2 113
cittecla 31:51f52ffa4b51 114 if(direction && speed) {
cittecla 20:859c7aebf8a6 115 move_forward_medium(PID_correction_value);
cittecla 31:51f52ffa4b51 116 }
cittecla 20:859c7aebf8a6 117 if(direction && !speed) {
cittecla 29:e7d0208bf2af 118 float value = 1.0f-power_value_slow*PID_correction_value;
cittecla 29:e7d0208bf2af 119 move_forward_slow(value);
cittecla 31:51f52ffa4b51 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 31:51f52ffa4b51 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 31:51f52ffa4b51 135 */