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@21:cb40c0533bc2, 2017-03-02 (annotated)

Committer:
cittecla
Date:
Thu Mar 02 20:07:41 2017 +0000
Revision:
21:cb40c0533bc2
Parent:
20:859c7aebf8a6
Child:
22:c8e187b9d949
Synchronous test failed

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 21:cb40c0533bc2 19 DigitalOut led(LED1); // Board LED
cittecla 20:859c7aebf8a6 20
cittecla 19:baa8371d55b4 21 //motor stuff
cittecla 19:baa8371d55b4 22 DigitalOut enableMotorDriver(PB_2);
cittecla 19:baa8371d55b4 23 PwmOut pwmL(PA_8);
cittecla 19:baa8371d55b4 24 PwmOut pwmR(PA_9);
cittecla 21:cb40c0533bc2 25 DigitalIn motorDriverFault(PB_14);
cittecla 21:cb40c0533bc2 26 DigitalIn motorDriverWarning(PB_15);
cittecla 21:cb40c0533bc2 27
cittecla 21:cb40c0533bc2 28 InterruptIn EncoderLeftA(PB_6);
cittecla 21:cb40c0533bc2 29 InterruptIn EncoderLeftB(PB_7);
cittecla 21:cb40c0533bc2 30 InterruptIn EncoderRightA(PA_6);
cittecla 21:cb40c0533bc2 31 InterruptIn EncoderRightB(PA_7);
cittecla 21:cb40c0533bc2 32
cittecla 21:cb40c0533bc2 33 static int EncoderCounterLeft = 0;
cittecla 21:cb40c0533bc2 34 static int EncoderCounterRight = 0;
cittecla 21:cb40c0533bc2 35
cittecla 18:a82994e67297 36
cittecla 18:a82994e67297 37 void move_init()
cittecla 18:a82994e67297 38 {
cittecla 19:baa8371d55b4 39 pwmL.period(0.00005f); // Setzt die Periode auf 50 μs
cittecla 19:baa8371d55b4 40 pwmR.period(0.00005f);
cittecla 19:baa8371d55b4 41
cittecla 19:baa8371d55b4 42 pwmL = 0.5f; // Setzt die Duty-Cycle auf 50%
cittecla 19:baa8371d55b4 43 pwmR = 0.5f;
cittecla 21:cb40c0533bc2 44 enableMotorDriver = 1;
cittecla 21:cb40c0533bc2 45
cittecla 21:cb40c0533bc2 46 EncoderCounterLeft = 0;
cittecla 21:cb40c0533bc2 47 EncoderCounterRight = 0;
cittecla 21:cb40c0533bc2 48 PID_correction_value = 1.0f;
cittecla 18:a82994e67297 49 }
cittecla 18:a82994e67297 50
cittecla 20:859c7aebf8a6 51 void move_forward_slow(float correction_value)
cittecla 18:a82994e67297 52 {
cittecla 19:baa8371d55b4 53 pwmL = power_value_slow;
cittecla 19:baa8371d55b4 54 pwmR = 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_forward_fast(float correction_value)
cittecla 18:a82994e67297 64 {
cittecla 19:baa8371d55b4 65 pwmL = power_value_fast;
cittecla 19:baa8371d55b4 66 pwmR = 1-power_value_fast*correction_value;
cittecla 18:a82994e67297 67 }
cittecla 18:a82994e67297 68
cittecla 20:859c7aebf8a6 69 void move_backward_slow(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 18:a82994e67297 73 }
cittecla 18:a82994e67297 74
cittecla 20:859c7aebf8a6 75 void move_backward_medium(float correction_value)
cittecla 18:a82994e67297 76 {
cittecla 19:baa8371d55b4 77 pwmL = 1-power_value_slow*correction_value;
cittecla 19:baa8371d55b4 78 pwmR = power_value_slow;
cittecla 18:a82994e67297 79 }
cittecla 18:a82994e67297 80
cittecla 20:859c7aebf8a6 81 void move_backward_fast(float correction_value)
cittecla 18:a82994e67297 82 {
cittecla 19:baa8371d55b4 83 pwmL = 1-power_value_slow*correction_value;
cittecla 19:baa8371d55b4 84 pwmR = power_value_slow;
cittecla 19:baa8371d55b4 85 }
cittecla 18:a82994e67297 86
cittecla 19:baa8371d55b4 87 void stop_movement()
cittecla 19:baa8371d55b4 88 {
cittecla 21:cb40c0533bc2 89 pwmL = 0.5f;
cittecla 21:cb40c0533bc2 90 pwmR = 0.5f;
cittecla 18:a82994e67297 91 }
cittecla 18:a82994e67297 92
cittecla 21:cb40c0533bc2 93 void highPulseDetectedL()
cittecla 21:cb40c0533bc2 94 {
cittecla 21:cb40c0533bc2 95 EncoderCounterLeft += 1;
cittecla 21:cb40c0533bc2 96 }
cittecla 21:cb40c0533bc2 97
cittecla 21:cb40c0533bc2 98 void highPulseDetectedR()
cittecla 21:cb40c0533bc2 99 {
cittecla 21:cb40c0533bc2 100 EncoderCounterRight +=1;
cittecla 21:cb40c0533bc2 101 }
cittecla 18:a82994e67297 102
cittecla 20:859c7aebf8a6 103
cittecla 20:859c7aebf8a6 104
cittecla 20:859c7aebf8a6 105
cittecla 21:cb40c0533bc2 106 void sync_movement(bool speed, bool direction)
cittecla 21:cb40c0533bc2 107 {
cittecla 21:cb40c0533bc2 108
cittecla 21:cb40c0533bc2 109 EncoderLeftA.rise(&highPulseDetectedL);
cittecla 21:cb40c0533bc2 110 EncoderLeftB.rise(&highPulseDetectedL);
cittecla 21:cb40c0533bc2 111 EncoderRightA.rise(&highPulseDetectedR);
cittecla 21:cb40c0533bc2 112 EncoderRightB.rise(&highPulseDetectedR);
cittecla 20:859c7aebf8a6 113
cittecla 20:859c7aebf8a6 114
cittecla 21:cb40c0533bc2 115 // PID correction Value calcualtion
cittecla 20:859c7aebf8a6 116
cittecla 21:cb40c0533bc2 117 if(EncoderCounterLeft > EncoderCounterRight) {
cittecla 21:cb40c0533bc2 118 PID_correction_value -= 0.0001f;
cittecla 21:cb40c0533bc2 119 led = 1;
cittecla 21:cb40c0533bc2 120 } else {
cittecla 21:cb40c0533bc2 121 led = 0;
cittecla 21:cb40c0533bc2 122 PID_correction_value += 0.0001f;
cittecla 21:cb40c0533bc2 123 }
cittecla 20:859c7aebf8a6 124
cittecla 21:cb40c0533bc2 125 if(PID_correction_value < 0.0f) {
cittecla 21:cb40c0533bc2 126 PID_correction_value = 0;
cittecla 21:cb40c0533bc2 127 }
cittecla 21:cb40c0533bc2 128 if(PID_correction_value > 2.0f) {
cittecla 21:cb40c0533bc2 129 PID_correction_value = 2;
cittecla 21:cb40c0533bc2 130 }
cittecla 21:cb40c0533bc2 131
cittecla 20:859c7aebf8a6 132 // Call movement:
cittecla 20:859c7aebf8a6 133 // direction 0 = backward, direction 1 = forward
cittecla 20:859c7aebf8a6 134 // speed 0 = slow, speed 1 = medium
cittecla 21:cb40c0533bc2 135
cittecla 20:859c7aebf8a6 136 if(direction && speed) {
cittecla 20:859c7aebf8a6 137 move_forward_medium(PID_correction_value);
cittecla 20:859c7aebf8a6 138 }
cittecla 20:859c7aebf8a6 139 if(direction && !speed) {
cittecla 20:859c7aebf8a6 140 move_forward_slow(PID_correction_value);
cittecla 20:859c7aebf8a6 141 }
cittecla 20:859c7aebf8a6 142 if(!direction && speed) {
cittecla 20:859c7aebf8a6 143 move_backward_medium(PID_correction_value);
cittecla 20:859c7aebf8a6 144 }
cittecla 21:cb40c0533bc2 145 if(!direction && !speed) {
cittecla 20:859c7aebf8a6 146 move_backward_slow(PID_correction_value);
cittecla 20:859c7aebf8a6 147 }
cittecla 20:859c7aebf8a6 148 }
cittecla 20:859c7aebf8a6 149
cittecla 21:cb40c0533bc2 150 void termiante_sync_movement()
cittecla 21:cb40c0533bc2 151 {
cittecla 20:859c7aebf8a6 152 PID_correction_value = 1.0f;
cittecla 21:cb40c0533bc2 153 }
cittecla 20:859c7aebf8a6 154