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@24:6c2fec64f890, 2017-03-04 (annotated)

Committer:
cittecla
Date:
Sat Mar 04 15:04:14 2017 +0000
Revision:
24:6c2fec64f890
Parent:
23:4ddc4216f335
Child:
25:08ee4525155b
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 21:cb40c0533bc2 31
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 21:cb40c0533bc2 91 void highPulseDetectedL()
cittecla 21:cb40c0533bc2 92 {
cittecla 21:cb40c0533bc2 93 EncoderCounterLeft += 1;
cittecla 24:6c2fec64f890 94 //led = 1;
cittecla 21:cb40c0533bc2 95 }
cittecla 21:cb40c0533bc2 96
cittecla 21:cb40c0533bc2 97 void highPulseDetectedR()
cittecla 21:cb40c0533bc2 98 {
cittecla 21:cb40c0533bc2 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 23:4ddc4216f335 105 /* if(EncoderLeftA) {
cittecla 23:4ddc4216f335 106 led = 1;
cittecla 23:4ddc4216f335 107 } else {
cittecla 23:4ddc4216f335 108 led = 0;
cittecla 23:4ddc4216f335 109 }*/
cittecla 20:859c7aebf8a6 110
cittecla 20:859c7aebf8a6 111
cittecla 21:cb40c0533bc2 112 // PID correction Value calcualtion
cittecla 20:859c7aebf8a6 113
cittecla 23:4ddc4216f335 114 if(EncoderCounterLeft > EncoderCounterRight) {
cittecla 23:4ddc4216f335 115 PID_correction_value += 0.0001f;
cittecla 22:c8e187b9d949 116
cittecla 23:4ddc4216f335 117 printf("Left higher");
cittecla 23:4ddc4216f335 118 } else {
cittecla 23:4ddc4216f335 119 if(EncoderCounterLeft < EncoderCounterRight) {
cittecla 23:4ddc4216f335 120 PID_correction_value -= 0.0001f;
cittecla 23:4ddc4216f335 121 printf("Right higher");
cittecla 23:4ddc4216f335 122 } else {
cittecla 23:4ddc4216f335 123 printf("Even");
cittecla 23:4ddc4216f335 124 }
cittecla 23:4ddc4216f335 125 }
cittecla 22:c8e187b9d949 126
cittecla 22:c8e187b9d949 127 /* if(PID_correction_value < 0.0f) {
cittecla 22:c8e187b9d949 128 PID_correction_value = 0;
cittecla 22:c8e187b9d949 129 }
cittecla 22:c8e187b9d949 130 if(PID_correction_value > 2.0f) {
cittecla 22:c8e187b9d949 131 PID_correction_value = 2;
cittecla 22:c8e187b9d949 132 }*/
cittecla 22:c8e187b9d949 133
cittecla 20:859c7aebf8a6 134 // Call movement:
cittecla 20:859c7aebf8a6 135 // direction 0 = backward, direction 1 = forward
cittecla 20:859c7aebf8a6 136 // speed 0 = slow, speed 1 = medium
cittecla 21:cb40c0533bc2 137
cittecla 20:859c7aebf8a6 138 if(direction && speed) {
cittecla 20:859c7aebf8a6 139 move_forward_medium(PID_correction_value);
cittecla 20:859c7aebf8a6 140 }
cittecla 20:859c7aebf8a6 141 if(direction && !speed) {
cittecla 20:859c7aebf8a6 142 move_forward_slow(PID_correction_value);
cittecla 20:859c7aebf8a6 143 }
cittecla 20:859c7aebf8a6 144 if(!direction && speed) {
cittecla 20:859c7aebf8a6 145 move_backward_medium(PID_correction_value);
cittecla 20:859c7aebf8a6 146 }
cittecla 21:cb40c0533bc2 147 if(!direction && !speed) {
cittecla 22:c8e187b9d949 148 move_backward_slow(PID_correction_value);
cittecla 20:859c7aebf8a6 149 }
cittecla 20:859c7aebf8a6 150 }
cittecla 20:859c7aebf8a6 151
cittecla 21:cb40c0533bc2 152 void termiante_sync_movement()
cittecla 21:cb40c0533bc2 153 {
cittecla 20:859c7aebf8a6 154 PID_correction_value = 1.0f;
cittecla 21:cb40c0533bc2 155 }
cittecla 20:859c7aebf8a6 156