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/Robot.cpp@57:1a395b6928ee, 2017-04-18 (annotated)

Committer:
cittecla
Date:
Tue Apr 18 15:38:15 2017 +0000
Revision:
57:1a395b6928ee
Parent:
43:9f291a496db8
Child:
58:a78103b0bf2a
update start_up

Who changed what in which revision?

UserRevisionLine numberNew contents of line
cittecla 34:40d8d29b44b8 1 // Roboter file
cittecla 34:40d8d29b44b8 2
cittecla 34:40d8d29b44b8 3
cittecla 34:40d8d29b44b8 4 #include "mbed.h"
cittecla 34:40d8d29b44b8 5 #include "Robot.h"
cittecla 34:40d8d29b44b8 6 #include "EncoderCounter.h"
cittecla 34:40d8d29b44b8 7 #include "LowpassFilter.h"
cittecla 34:40d8d29b44b8 8 #include "IMU.h"
cittecla 34:40d8d29b44b8 9
cittecla 34:40d8d29b44b8 10 #include "IRSensor.h"
cittecla 34:40d8d29b44b8 11
cittecla 34:40d8d29b44b8 12
cittecla 34:40d8d29b44b8 13 const float PERIOD = 0.001f; // period of control task, given in [s]
cittecla 34:40d8d29b44b8 14 const float COUNTS_PER_TURN = 1200.0f; // resolution of encoder counter
cittecla 34:40d8d29b44b8 15 const float LOWPASS_FILTER_FREQUENCY = 300.0f; // frequency of lowpass filter for actual speed values, given in [rad/s]
cittecla 34:40d8d29b44b8 16 const float KN = 40.0f; // speed constant of motor, given in [rpm/V]
cittecla 34:40d8d29b44b8 17 const float KP = 0.2f; // speed controller gain, given in [V/rpm]
cittecla 34:40d8d29b44b8 18 const float MAX_VOLTAGE = 12.0f; // supply voltage for power stage in [V]
cittecla 34:40d8d29b44b8 19 const float MIN_DUTY_CYCLE = 0.02f; // minimum allowed value for duty cycle (2%)
cittecla 34:40d8d29b44b8 20 const float MAX_DUTY_CYCLE = 0.98f; // maximum allowed value for duty cycle (98%)
cittecla 34:40d8d29b44b8 21 const float correction_value = 2.45f; // correction value for desired speed
aeschsim 43:9f291a496db8 22
cittecla 34:40d8d29b44b8 23 LowpassFilter speedLeftFilter;
cittecla 34:40d8d29b44b8 24 LowpassFilter speedRightFilter;
cittecla 34:40d8d29b44b8 25
cittecla 34:40d8d29b44b8 26 //Motor stuff
cittecla 34:40d8d29b44b8 27 EncoderCounter counterLeft(PB_6, PB_7);
cittecla 34:40d8d29b44b8 28 EncoderCounter counterRight(PA_6, PC_7);
cittecla 34:40d8d29b44b8 29
cittecla 34:40d8d29b44b8 30 DigitalOut enableMotorDriver(PB_2);
cittecla 34:40d8d29b44b8 31 PwmOut pwmLeft(PA_8);
cittecla 34:40d8d29b44b8 32 PwmOut pwmRight(PA_9);
cittecla 34:40d8d29b44b8 33
cittecla 34:40d8d29b44b8 34 DigitalOut my_led(LED1);
cittecla 34:40d8d29b44b8 35
cittecla 34:40d8d29b44b8 36 //Periphery for distance sensors
cittecla 34:40d8d29b44b8 37 AnalogIn distance(PB_1);
cittecla 34:40d8d29b44b8 38 DigitalOut enable(PC_1);
cittecla 34:40d8d29b44b8 39 DigitalOut bit0(PH_1);
cittecla 34:40d8d29b44b8 40 DigitalOut bit1(PC_2);
cittecla 34:40d8d29b44b8 41 DigitalOut bit2(PC_3);
cittecla 34:40d8d29b44b8 42
cittecla 34:40d8d29b44b8 43 IRSensor sensors[6];
cittecla 34:40d8d29b44b8 44
cittecla 34:40d8d29b44b8 45 //indicator leds arround robot
cittecla 34:40d8d29b44b8 46 DigitalOut leds[] = { PC_8, PC_6, PB_12, PA_7, PC_0, PC_9 };
cittecla 34:40d8d29b44b8 47
cittecla 34:40d8d29b44b8 48
cittecla 34:40d8d29b44b8 49 //Periphery for the IMU
cittecla 34:40d8d29b44b8 50 SPI spi(PC_12, PC_11, PC_10);
cittecla 34:40d8d29b44b8 51 DigitalOut csG(PA_15);
cittecla 34:40d8d29b44b8 52 DigitalOut csXM(PD_2);
cittecla 34:40d8d29b44b8 53
cittecla 34:40d8d29b44b8 54 IMU imu(spi, csG, csXM);
cittecla 34:40d8d29b44b8 55
cittecla 34:40d8d29b44b8 56 //Periphery for I2C
cittecla 34:40d8d29b44b8 57 I2C i2c(PB_9, PB_8);
cittecla 34:40d8d29b44b8 58 DigitalOut Servo_enable(PA_10);
cittecla 34:40d8d29b44b8 59
aeschsim 43:9f291a496db8 60 uint8_t mode1 = 0x00;
aeschsim 43:9f291a496db8 61 uint8_t mode2 = 0x01;
aeschsim 43:9f291a496db8 62 uint8_t LED0_ON_L = 0x06;
aeschsim 43:9f291a496db8 63 uint8_t LED0_ON_H = 0x07;
aeschsim 43:9f291a496db8 64 uint8_t LED0_OFF_L = 0x08;
aeschsim 43:9f291a496db8 65 uint8_t LED0_OFF_H = 0x09;
aeschsim 43:9f291a496db8 66 uint8_t ALL_LED_ON_L = 0xFA;
aeschsim 43:9f291a496db8 67 uint8_t ALL_LED_ON_H = 0xFB;
aeschsim 43:9f291a496db8 68 uint8_t ALL_LED_OFF_L = 0xFC;
aeschsim 43:9f291a496db8 69 uint8_t ALL_LED_OFF_H = 0xFD;
aeschsim 43:9f291a496db8 70
cittecla 34:40d8d29b44b8 71
cittecla 34:40d8d29b44b8 72 short previousValueCounterRight = 0;
cittecla 34:40d8d29b44b8 73 short previousValueCounterLeft = 0;
cittecla 34:40d8d29b44b8 74
cittecla 34:40d8d29b44b8 75 float desiredSpeedLeft = 0;
cittecla 34:40d8d29b44b8 76 float desiredSpeedRight = 0;
cittecla 34:40d8d29b44b8 77
cittecla 34:40d8d29b44b8 78 float actualSpeedLeft;
cittecla 34:40d8d29b44b8 79 float actualSpeedRight;
cittecla 34:40d8d29b44b8 80
cittecla 34:40d8d29b44b8 81
cittecla 34:40d8d29b44b8 82
cittecla 34:40d8d29b44b8 83
cittecla 34:40d8d29b44b8 84 void Robot_init_all()
cittecla 34:40d8d29b44b8 85 {
cittecla 34:40d8d29b44b8 86 Speedcontroller_init();
cittecla 34:40d8d29b44b8 87 Sensor_init();
cittecla 57:1a395b6928ee 88 init_servo(60);
cittecla 34:40d8d29b44b8 89 }
cittecla 34:40d8d29b44b8 90
cittecla 34:40d8d29b44b8 91 //speed controll
cittecla 34:40d8d29b44b8 92 void Speedcontroller_init()
cittecla 34:40d8d29b44b8 93 {
cittecla 34:40d8d29b44b8 94 // Initialisieren der PWM Ausgaenge pwmLeft.period(0.00005f); // PWM Periode von 50 us
cittecla 34:40d8d29b44b8 95 pwmLeft.period(0.00005f); // Setzt die Periode auf 50 μs
cittecla 34:40d8d29b44b8 96 pwmRight.period(0.00005f);
cittecla 34:40d8d29b44b8 97 pwmLeft = 0.5f; // Duty-Cycle von 50% pwmRight.period(0.00005f); // PWM Periode von 50 us
cittecla 34:40d8d29b44b8 98 pwmRight = 0.5f; // Duty-Cycle von 50%
cittecla 34:40d8d29b44b8 99
cittecla 34:40d8d29b44b8 100 // Initialisieren von lokalen Variabeln
cittecla 34:40d8d29b44b8 101 previousValueCounterLeft = counterLeft.read();
cittecla 34:40d8d29b44b8 102 previousValueCounterRight = counterRight.read();
cittecla 34:40d8d29b44b8 103 speedLeftFilter.setPeriod(PERIOD);
cittecla 34:40d8d29b44b8 104 speedLeftFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
cittecla 34:40d8d29b44b8 105 speedRightFilter.setPeriod(PERIOD);
cittecla 34:40d8d29b44b8 106 speedRightFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
cittecla 34:40d8d29b44b8 107
cittecla 34:40d8d29b44b8 108 desiredSpeedLeft = 0.0f;
cittecla 34:40d8d29b44b8 109 desiredSpeedRight = 0.0f;
cittecla 34:40d8d29b44b8 110 actualSpeedLeft = 0.0f;
cittecla 34:40d8d29b44b8 111 actualSpeedRight = 0.0f;
cittecla 34:40d8d29b44b8 112
cittecla 34:40d8d29b44b8 113 Ticker t1;
cittecla 34:40d8d29b44b8 114 t1.attach( &speedCtrl, PERIOD);
cittecla 34:40d8d29b44b8 115
cittecla 34:40d8d29b44b8 116 //desiredSpeedLeft = 50.0f+correction_value; //50 RPM
cittecla 34:40d8d29b44b8 117 //desiredSpeedRight = -50.0f; //50 RPM
cittecla 34:40d8d29b44b8 118 enableMotorDriver = 1;
cittecla 34:40d8d29b44b8 119 }
cittecla 34:40d8d29b44b8 120
cittecla 34:40d8d29b44b8 121 void set_speed(float left, float right)
cittecla 34:40d8d29b44b8 122 {
cittecla 34:40d8d29b44b8 123 desiredSpeedLeft = left+correction_value; //50 RPM
cittecla 34:40d8d29b44b8 124 desiredSpeedRight = -right; //50 RPM
cittecla 34:40d8d29b44b8 125 }
cittecla 34:40d8d29b44b8 126
cittecla 34:40d8d29b44b8 127
cittecla 34:40d8d29b44b8 128 void speedCtrl()
cittecla 34:40d8d29b44b8 129 {
cittecla 34:40d8d29b44b8 130 // Berechnen die effektiven Drehzahlen der Motoren in [rpm]
cittecla 34:40d8d29b44b8 131 short valueCounterLeft = counterLeft.read();
cittecla 34:40d8d29b44b8 132 short valueCounterRight = counterRight.read();
cittecla 34:40d8d29b44b8 133 short countsInPastPeriodLeft = valueCounterLeft-previousValueCounterLeft;
cittecla 34:40d8d29b44b8 134 short countsInPastPeriodRight = valueCounterRight-previousValueCounterRight;
cittecla 34:40d8d29b44b8 135
cittecla 34:40d8d29b44b8 136 previousValueCounterLeft = valueCounterLeft;
cittecla 34:40d8d29b44b8 137 previousValueCounterRight = valueCounterRight;
cittecla 34:40d8d29b44b8 138 actualSpeedLeft = speedLeftFilter.filter((float)countsInPastPeriodLeft /COUNTS_PER_TURN/PERIOD*60.0f);
cittecla 34:40d8d29b44b8 139 actualSpeedRight = speedRightFilter.filter((float)countsInPastPeriodRight /COUNTS_PER_TURN/PERIOD*60.0f);
cittecla 34:40d8d29b44b8 140
cittecla 34:40d8d29b44b8 141 // Berechnen der Motorspannungen Uout
cittecla 34:40d8d29b44b8 142 float voltageLeft = KP*(desiredSpeedLeft-actualSpeedLeft)+desiredSpeedLeft/KN;
cittecla 34:40d8d29b44b8 143 float voltageRight = KP*(desiredSpeedRight-actualSpeedRight)+desiredSpeedRight/KN;
cittecla 34:40d8d29b44b8 144
cittecla 34:40d8d29b44b8 145 // Berechnen, Limitieren und Setzen der Duty-Cycle
cittecla 34:40d8d29b44b8 146 float dutyCycleLeft = 0.5f+0.5f*voltageLeft/MAX_VOLTAGE;
cittecla 34:40d8d29b44b8 147 if (dutyCycleLeft < MIN_DUTY_CYCLE) dutyCycleLeft = MIN_DUTY_CYCLE;
cittecla 34:40d8d29b44b8 148 else if (dutyCycleLeft > MAX_DUTY_CYCLE) dutyCycleLeft = MAX_DUTY_CYCLE;
cittecla 34:40d8d29b44b8 149
cittecla 34:40d8d29b44b8 150 pwmLeft = dutyCycleLeft;
cittecla 34:40d8d29b44b8 151 float dutyCycleRight = 0.5f+0.5f*voltageRight/MAX_VOLTAGE;
cittecla 34:40d8d29b44b8 152 if (dutyCycleRight < MIN_DUTY_CYCLE) dutyCycleRight = MIN_DUTY_CYCLE;
cittecla 34:40d8d29b44b8 153 else if (dutyCycleRight > MAX_DUTY_CYCLE) dutyCycleRight = MAX_DUTY_CYCLE;
cittecla 34:40d8d29b44b8 154
cittecla 34:40d8d29b44b8 155 pwmRight = dutyCycleRight;
cittecla 34:40d8d29b44b8 156 }
cittecla 34:40d8d29b44b8 157
cittecla 34:40d8d29b44b8 158 //Sensors
cittecla 34:40d8d29b44b8 159 void Sensor_init()
cittecla 34:40d8d29b44b8 160 {
cittecla 34:40d8d29b44b8 161 for( int ii = 0; ii<6; ++ii)
cittecla 34:40d8d29b44b8 162 sensors[ii].init(&distance, &bit0, &bit1, &bit2, ii);
cittecla 34:40d8d29b44b8 163 }
cittecla 34:40d8d29b44b8 164
cittecla 34:40d8d29b44b8 165 float getDistanceIR(int number)
cittecla 34:40d8d29b44b8 166 {
cittecla 34:40d8d29b44b8 167 return sensors[number];
cittecla 34:40d8d29b44b8 168 }
cittecla 34:40d8d29b44b8 169
cittecla 34:40d8d29b44b8 170 //IMU
cittecla 34:40d8d29b44b8 171 float read_acc_x()
cittecla 34:40d8d29b44b8 172 {
cittecla 34:40d8d29b44b8 173 return imu.readAccelerationX();
cittecla 34:40d8d29b44b8 174 }
cittecla 34:40d8d29b44b8 175
cittecla 34:40d8d29b44b8 176 float read_acc_y()
cittecla 34:40d8d29b44b8 177 {
cittecla 34:40d8d29b44b8 178 return imu.readAccelerationY();
cittecla 34:40d8d29b44b8 179 }
cittecla 34:40d8d29b44b8 180
cittecla 34:40d8d29b44b8 181 float read_acc_z()
cittecla 34:40d8d29b44b8 182 {
cittecla 34:40d8d29b44b8 183 return imu.readAccelerationZ();
cittecla 34:40d8d29b44b8 184 }
cittecla 34:40d8d29b44b8 185
cittecla 34:40d8d29b44b8 186 float read_gyr_x()
cittecla 34:40d8d29b44b8 187 {
cittecla 34:40d8d29b44b8 188 return imu.readGyroX();
cittecla 34:40d8d29b44b8 189 }
cittecla 34:40d8d29b44b8 190
cittecla 34:40d8d29b44b8 191 float read_gyr_y()
cittecla 34:40d8d29b44b8 192 {
cittecla 34:40d8d29b44b8 193 return imu.readGyroY();
cittecla 34:40d8d29b44b8 194 }
cittecla 34:40d8d29b44b8 195
cittecla 34:40d8d29b44b8 196 float read_gyr_z()
cittecla 34:40d8d29b44b8 197 {
cittecla 34:40d8d29b44b8 198 return imu.readGyroZ();
cittecla 34:40d8d29b44b8 199 }
cittecla 34:40d8d29b44b8 200
cittecla 34:40d8d29b44b8 201 float read_heading()
cittecla 34:40d8d29b44b8 202 {
cittecla 38:3526c36e4c73 203 double deg = (double)(180.0f/(double)M_PI*imu.readHeading());
cittecla 38:3526c36e4c73 204 return deg;
cittecla 34:40d8d29b44b8 205 }
cittecla 34:40d8d29b44b8 206
cittecla 34:40d8d29b44b8 207
cittecla 34:40d8d29b44b8 208 // Servo I2C
aeschsim 43:9f291a496db8 209 void init_servo(int freq) {
aeschsim 42:08b3aea29254 210 i2c.write(0x06); //0000 0110 - PCA reset
aeschsim 42:08b3aea29254 211 //freq *= 0.9;
aeschsim 42:08b3aea29254 212 float prescaleval = 25000000.0; //25MHz
aeschsim 43:9f291a496db8 213 prescaleval /= 4096.0f; //12-Bit
aeschsim 42:08b3aea29254 214 prescaleval /= (float)freq;
aeschsim 43:9f291a496db8 215 prescaleval -= 1.0f;
aeschsim 43:9f291a496db8 216 uint8_t prescale = floor(prescaleval +0.5f);
aeschsim 42:08b3aea29254 217
aeschsim 42:08b3aea29254 218 uint8_t oldmode = i2c.read(mode1);
aeschsim 42:08b3aea29254 219 uint8_t newmode = (oldmode&0x7f) | 0x10;
aeschsim 42:08b3aea29254 220 i2c.write(mode1<<8 + newmode);
aeschsim 42:08b3aea29254 221 i2c.write(0xFE<<8 + prescale); //0xFE - Prescale-Register
aeschsim 42:08b3aea29254 222 i2c.write(mode1<<8 + oldmode);
aeschsim 43:9f291a496db8 223 wait_ms(50);
aeschsim 42:08b3aea29254 224 i2c.write((mode1<<8) + (oldmode|0xA1));
aeschsim 42:08b3aea29254 225 }
aeschsim 42:08b3aea29254 226
aeschsim 42:08b3aea29254 227 void set_servo_position(int servo, int deg){
aeschsim 43:9f291a496db8 228 uint16_t on = 0x00;
aeschsim 43:9f291a496db8 229 uint16_t off;
aeschsim 43:9f291a496db8 230 off = 150 + (3 * deg); //deg von 0...150° abgebildet in min150...max600
aeschsim 43:9f291a496db8 231 i2c.write(LED0_ON_L+4*servo);
aeschsim 43:9f291a496db8 232 i2c.write(on);
aeschsim 43:9f291a496db8 233 i2c.write(on>>8);
aeschsim 43:9f291a496db8 234 i2c.write(off);
aeschsim 43:9f291a496db8 235 i2c.write(off>>8);
cittecla 34:40d8d29b44b8 236 }
cittecla 34:40d8d29b44b8 237
cittecla 34:40d8d29b44b8 238 void enable_servos(){
cittecla 34:40d8d29b44b8 239 Servo_enable = 1;
cittecla 34:40d8d29b44b8 240 }
cittecla 34:40d8d29b44b8 241
cittecla 34:40d8d29b44b8 242 void disable_servos(){
cittecla 34:40d8d29b44b8 243 Servo_enable = 0;
cittecla 34:40d8d29b44b8 244 }