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@58:a78103b0bf2a, 2017-04-18 (annotated)

Committer:
cittecla
Date:
Tue Apr 18 15:39:45 2017 +0000
Revision:
58:a78103b0bf2a
Parent:
57:1a395b6928ee
Parent:
55:a1e6fb87d648
Child:
60:b57577b0072f
merged

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
aeschsim 55:a1e6fb87d648 12 //Define I2C-Servoboard Consts
aeschsim 55:a1e6fb87d648 13 #define ADRESS 0x40
aeschsim 55:a1e6fb87d648 14 #define MODE1 0x00
aeschsim 55:a1e6fb87d648 15 #define MODE2 0x01
aeschsim 55:a1e6fb87d648 16 #define PRESCALE 0xFE
aeschsim 55:a1e6fb87d648 17 #define LED0_ON_L 0x06
aeschsim 55:a1e6fb87d648 18 #define LED0_ON_H 0x07
aeschsim 55:a1e6fb87d648 19 #define LED0_OFF_L 0x08
aeschsim 55:a1e6fb87d648 20
aeschsim 55:a1e6fb87d648 21 #define RESTART 0x80
aeschsim 55:a1e6fb87d648 22 #define SLEEP 0x10
aeschsim 55:a1e6fb87d648 23
cittecla 34:40d8d29b44b8 24
cittecla 34:40d8d29b44b8 25 const float PERIOD = 0.001f; // period of control task, given in [s]
cittecla 34:40d8d29b44b8 26 const float COUNTS_PER_TURN = 1200.0f; // resolution of encoder counter
cittecla 34:40d8d29b44b8 27 const float LOWPASS_FILTER_FREQUENCY = 300.0f; // frequency of lowpass filter for actual speed values, given in [rad/s]
cittecla 34:40d8d29b44b8 28 const float KN = 40.0f; // speed constant of motor, given in [rpm/V]
cittecla 34:40d8d29b44b8 29 const float KP = 0.2f; // speed controller gain, given in [V/rpm]
cittecla 34:40d8d29b44b8 30 const float MAX_VOLTAGE = 12.0f; // supply voltage for power stage in [V]
cittecla 34:40d8d29b44b8 31 const float MIN_DUTY_CYCLE = 0.02f; // minimum allowed value for duty cycle (2%)
cittecla 34:40d8d29b44b8 32 const float MAX_DUTY_CYCLE = 0.98f; // maximum allowed value for duty cycle (98%)
cittecla 34:40d8d29b44b8 33 const float correction_value = 2.45f; // correction value for desired speed
aeschsim 43:9f291a496db8 34
cittecla 34:40d8d29b44b8 35 LowpassFilter speedLeftFilter;
cittecla 34:40d8d29b44b8 36 LowpassFilter speedRightFilter;
cittecla 34:40d8d29b44b8 37
aeschsim 55:a1e6fb87d648 38 DigitalIn user(USER_BUTTON);
aeschsim 55:a1e6fb87d648 39
cittecla 34:40d8d29b44b8 40 //Motor stuff
cittecla 34:40d8d29b44b8 41 EncoderCounter counterLeft(PB_6, PB_7);
cittecla 34:40d8d29b44b8 42 EncoderCounter counterRight(PA_6, PC_7);
cittecla 34:40d8d29b44b8 43
cittecla 34:40d8d29b44b8 44 DigitalOut enableMotorDriver(PB_2);
cittecla 34:40d8d29b44b8 45 PwmOut pwmLeft(PA_8);
cittecla 34:40d8d29b44b8 46 PwmOut pwmRight(PA_9);
cittecla 34:40d8d29b44b8 47
cittecla 34:40d8d29b44b8 48 DigitalOut my_led(LED1);
cittecla 34:40d8d29b44b8 49
cittecla 34:40d8d29b44b8 50 //Periphery for distance sensors
cittecla 34:40d8d29b44b8 51 AnalogIn distance(PB_1);
aeschsim 55:a1e6fb87d648 52 AnalogIn distance2(PA_1);
cittecla 34:40d8d29b44b8 53 DigitalOut enable(PC_1);
cittecla 34:40d8d29b44b8 54 DigitalOut bit0(PH_1);
cittecla 34:40d8d29b44b8 55 DigitalOut bit1(PC_2);
cittecla 34:40d8d29b44b8 56 DigitalOut bit2(PC_3);
cittecla 34:40d8d29b44b8 57
cittecla 34:40d8d29b44b8 58 IRSensor sensors[6];
cittecla 34:40d8d29b44b8 59
cittecla 34:40d8d29b44b8 60 //indicator leds arround robot
cittecla 34:40d8d29b44b8 61 DigitalOut leds[] = { PC_8, PC_6, PB_12, PA_7, PC_0, PC_9 };
cittecla 34:40d8d29b44b8 62
cittecla 34:40d8d29b44b8 63
cittecla 34:40d8d29b44b8 64 //Periphery for the IMU
cittecla 34:40d8d29b44b8 65 SPI spi(PC_12, PC_11, PC_10);
cittecla 34:40d8d29b44b8 66 DigitalOut csG(PA_15);
cittecla 34:40d8d29b44b8 67 DigitalOut csXM(PD_2);
cittecla 34:40d8d29b44b8 68
cittecla 34:40d8d29b44b8 69 IMU imu(spi, csG, csXM);
cittecla 34:40d8d29b44b8 70
cittecla 34:40d8d29b44b8 71 //Periphery for I2C
cittecla 34:40d8d29b44b8 72 I2C i2c(PB_9, PB_8);
cittecla 34:40d8d29b44b8 73 DigitalOut Servo_enable(PA_10);
cittecla 34:40d8d29b44b8 74
aeschsim 55:a1e6fb87d648 75 //Color sensor
aeschsim 55:a1e6fb87d648 76 DigitalIn Red(PB_13);
aeschsim 55:a1e6fb87d648 77 DigitalIn Green(PC_4);
cittecla 34:40d8d29b44b8 78
cittecla 34:40d8d29b44b8 79 short previousValueCounterRight = 0;
cittecla 34:40d8d29b44b8 80 short previousValueCounterLeft = 0;
cittecla 34:40d8d29b44b8 81
cittecla 34:40d8d29b44b8 82 float desiredSpeedLeft = 0;
cittecla 34:40d8d29b44b8 83 float desiredSpeedRight = 0;
cittecla 34:40d8d29b44b8 84
cittecla 34:40d8d29b44b8 85 float actualSpeedLeft;
cittecla 34:40d8d29b44b8 86 float actualSpeedRight;
cittecla 34:40d8d29b44b8 87
cittecla 34:40d8d29b44b8 88
aeschsim 55:a1e6fb87d648 89 bool get_user()
aeschsim 55:a1e6fb87d648 90 {
aeschsim 55:a1e6fb87d648 91 return user;
aeschsim 55:a1e6fb87d648 92 }
cittecla 34:40d8d29b44b8 93
cittecla 34:40d8d29b44b8 94
cittecla 34:40d8d29b44b8 95 void Robot_init_all()
cittecla 34:40d8d29b44b8 96 {
cittecla 34:40d8d29b44b8 97 Speedcontroller_init();
cittecla 34:40d8d29b44b8 98 Sensor_init();
cittecla 57:1a395b6928ee 99 init_servo(60);
cittecla 34:40d8d29b44b8 100 }
cittecla 34:40d8d29b44b8 101
cittecla 34:40d8d29b44b8 102 //speed controll
cittecla 34:40d8d29b44b8 103 void Speedcontroller_init()
cittecla 34:40d8d29b44b8 104 {
cittecla 34:40d8d29b44b8 105 // Initialisieren der PWM Ausgaenge pwmLeft.period(0.00005f); // PWM Periode von 50 us
cittecla 34:40d8d29b44b8 106 pwmLeft.period(0.00005f); // Setzt die Periode auf 50 μs
cittecla 34:40d8d29b44b8 107 pwmRight.period(0.00005f);
cittecla 34:40d8d29b44b8 108 pwmLeft = 0.5f; // Duty-Cycle von 50% pwmRight.period(0.00005f); // PWM Periode von 50 us
cittecla 34:40d8d29b44b8 109 pwmRight = 0.5f; // Duty-Cycle von 50%
cittecla 34:40d8d29b44b8 110
cittecla 34:40d8d29b44b8 111 // Initialisieren von lokalen Variabeln
cittecla 34:40d8d29b44b8 112 previousValueCounterLeft = counterLeft.read();
cittecla 34:40d8d29b44b8 113 previousValueCounterRight = counterRight.read();
cittecla 34:40d8d29b44b8 114 speedLeftFilter.setPeriod(PERIOD);
cittecla 34:40d8d29b44b8 115 speedLeftFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
cittecla 34:40d8d29b44b8 116 speedRightFilter.setPeriod(PERIOD);
cittecla 34:40d8d29b44b8 117 speedRightFilter.setFrequency(LOWPASS_FILTER_FREQUENCY);
cittecla 34:40d8d29b44b8 118
cittecla 34:40d8d29b44b8 119 desiredSpeedLeft = 0.0f;
cittecla 34:40d8d29b44b8 120 desiredSpeedRight = 0.0f;
cittecla 34:40d8d29b44b8 121 actualSpeedLeft = 0.0f;
cittecla 34:40d8d29b44b8 122 actualSpeedRight = 0.0f;
cittecla 34:40d8d29b44b8 123
cittecla 34:40d8d29b44b8 124 Ticker t1;
cittecla 34:40d8d29b44b8 125 t1.attach( &speedCtrl, PERIOD);
cittecla 34:40d8d29b44b8 126
cittecla 34:40d8d29b44b8 127 //desiredSpeedLeft = 50.0f+correction_value; //50 RPM
cittecla 34:40d8d29b44b8 128 //desiredSpeedRight = -50.0f; //50 RPM
cittecla 34:40d8d29b44b8 129 enableMotorDriver = 1;
cittecla 34:40d8d29b44b8 130 }
cittecla 34:40d8d29b44b8 131
cittecla 34:40d8d29b44b8 132 void set_speed(float left, float right)
cittecla 34:40d8d29b44b8 133 {
cittecla 34:40d8d29b44b8 134 desiredSpeedLeft = left+correction_value; //50 RPM
cittecla 34:40d8d29b44b8 135 desiredSpeedRight = -right; //50 RPM
cittecla 34:40d8d29b44b8 136 }
cittecla 34:40d8d29b44b8 137
cittecla 34:40d8d29b44b8 138
cittecla 34:40d8d29b44b8 139 void speedCtrl()
cittecla 34:40d8d29b44b8 140 {
cittecla 34:40d8d29b44b8 141 // Berechnen die effektiven Drehzahlen der Motoren in [rpm]
cittecla 34:40d8d29b44b8 142 short valueCounterLeft = counterLeft.read();
cittecla 34:40d8d29b44b8 143 short valueCounterRight = counterRight.read();
cittecla 34:40d8d29b44b8 144 short countsInPastPeriodLeft = valueCounterLeft-previousValueCounterLeft;
cittecla 34:40d8d29b44b8 145 short countsInPastPeriodRight = valueCounterRight-previousValueCounterRight;
cittecla 34:40d8d29b44b8 146
cittecla 34:40d8d29b44b8 147 previousValueCounterLeft = valueCounterLeft;
cittecla 34:40d8d29b44b8 148 previousValueCounterRight = valueCounterRight;
cittecla 34:40d8d29b44b8 149 actualSpeedLeft = speedLeftFilter.filter((float)countsInPastPeriodLeft /COUNTS_PER_TURN/PERIOD*60.0f);
cittecla 34:40d8d29b44b8 150 actualSpeedRight = speedRightFilter.filter((float)countsInPastPeriodRight /COUNTS_PER_TURN/PERIOD*60.0f);
cittecla 34:40d8d29b44b8 151
cittecla 34:40d8d29b44b8 152 // Berechnen der Motorspannungen Uout
cittecla 34:40d8d29b44b8 153 float voltageLeft = KP*(desiredSpeedLeft-actualSpeedLeft)+desiredSpeedLeft/KN;
cittecla 34:40d8d29b44b8 154 float voltageRight = KP*(desiredSpeedRight-actualSpeedRight)+desiredSpeedRight/KN;
cittecla 34:40d8d29b44b8 155
cittecla 34:40d8d29b44b8 156 // Berechnen, Limitieren und Setzen der Duty-Cycle
cittecla 34:40d8d29b44b8 157 float dutyCycleLeft = 0.5f+0.5f*voltageLeft/MAX_VOLTAGE;
cittecla 34:40d8d29b44b8 158 if (dutyCycleLeft < MIN_DUTY_CYCLE) dutyCycleLeft = MIN_DUTY_CYCLE;
cittecla 34:40d8d29b44b8 159 else if (dutyCycleLeft > MAX_DUTY_CYCLE) dutyCycleLeft = MAX_DUTY_CYCLE;
cittecla 34:40d8d29b44b8 160
cittecla 34:40d8d29b44b8 161 pwmLeft = dutyCycleLeft;
cittecla 34:40d8d29b44b8 162 float dutyCycleRight = 0.5f+0.5f*voltageRight/MAX_VOLTAGE;
cittecla 34:40d8d29b44b8 163 if (dutyCycleRight < MIN_DUTY_CYCLE) dutyCycleRight = MIN_DUTY_CYCLE;
cittecla 34:40d8d29b44b8 164 else if (dutyCycleRight > MAX_DUTY_CYCLE) dutyCycleRight = MAX_DUTY_CYCLE;
cittecla 34:40d8d29b44b8 165
cittecla 34:40d8d29b44b8 166 pwmRight = dutyCycleRight;
cittecla 34:40d8d29b44b8 167 }
cittecla 34:40d8d29b44b8 168
cittecla 34:40d8d29b44b8 169 //Sensors
cittecla 34:40d8d29b44b8 170 void Sensor_init()
cittecla 34:40d8d29b44b8 171 {
cittecla 34:40d8d29b44b8 172 for( int ii = 0; ii<6; ++ii)
cittecla 34:40d8d29b44b8 173 sensors[ii].init(&distance, &bit0, &bit1, &bit2, ii);
cittecla 34:40d8d29b44b8 174 }
cittecla 34:40d8d29b44b8 175
cittecla 34:40d8d29b44b8 176 float getDistanceIR(int number)
cittecla 34:40d8d29b44b8 177 {
cittecla 34:40d8d29b44b8 178 return sensors[number];
cittecla 34:40d8d29b44b8 179 }
cittecla 34:40d8d29b44b8 180
aeschsim 55:a1e6fb87d648 181 //Color
aeschsim 55:a1e6fb87d648 182 bool get_color() {
aeschsim 55:a1e6fb87d648 183 if (red == 0 && green == 1) {
aeschsim 55:a1e6fb87d648 184 return 1;
aeschsim 55:a1e6fb87d648 185 } else {
aeschsim 55:a1e6fb87d648 186 return 0;
aeschsim 55:a1e6fb87d648 187 }
aeschsim 55:a1e6fb87d648 188 }
aeschsim 55:a1e6fb87d648 189
cittecla 34:40d8d29b44b8 190 //IMU
cittecla 34:40d8d29b44b8 191 float read_acc_x()
cittecla 34:40d8d29b44b8 192 {
cittecla 34:40d8d29b44b8 193 return imu.readAccelerationX();
cittecla 34:40d8d29b44b8 194 }
cittecla 34:40d8d29b44b8 195
cittecla 34:40d8d29b44b8 196 float read_acc_y()
cittecla 34:40d8d29b44b8 197 {
cittecla 34:40d8d29b44b8 198 return imu.readAccelerationY();
cittecla 34:40d8d29b44b8 199 }
cittecla 34:40d8d29b44b8 200
cittecla 34:40d8d29b44b8 201 float read_acc_z()
cittecla 34:40d8d29b44b8 202 {
cittecla 34:40d8d29b44b8 203 return imu.readAccelerationZ();
cittecla 34:40d8d29b44b8 204 }
cittecla 34:40d8d29b44b8 205
cittecla 34:40d8d29b44b8 206 float read_gyr_x()
cittecla 34:40d8d29b44b8 207 {
cittecla 34:40d8d29b44b8 208 return imu.readGyroX();
cittecla 34:40d8d29b44b8 209 }
cittecla 34:40d8d29b44b8 210
cittecla 34:40d8d29b44b8 211 float read_gyr_y()
cittecla 34:40d8d29b44b8 212 {
cittecla 34:40d8d29b44b8 213 return imu.readGyroY();
cittecla 34:40d8d29b44b8 214 }
cittecla 34:40d8d29b44b8 215
cittecla 34:40d8d29b44b8 216 float read_gyr_z()
cittecla 34:40d8d29b44b8 217 {
cittecla 34:40d8d29b44b8 218 return imu.readGyroZ();
cittecla 34:40d8d29b44b8 219 }
cittecla 34:40d8d29b44b8 220
cittecla 34:40d8d29b44b8 221 float read_heading()
cittecla 34:40d8d29b44b8 222 {
cittecla 38:3526c36e4c73 223 double deg = (double)(180.0f/(double)M_PI*imu.readHeading());
cittecla 38:3526c36e4c73 224 return deg;
cittecla 34:40d8d29b44b8 225 }
cittecla 34:40d8d29b44b8 226
cittecla 34:40d8d29b44b8 227
cittecla 34:40d8d29b44b8 228 // Servo I2C
aeschsim 55:a1e6fb87d648 229
aeschsim 55:a1e6fb87d648 230
aeschsim 55:a1e6fb87d648 231 void init_servo(int freq)
aeschsim 55:a1e6fb87d648 232 {
aeschsim 55:a1e6fb87d648 233 char data[2];
aeschsim 55:a1e6fb87d648 234
aeschsim 55:a1e6fb87d648 235 //Reset
aeschsim 55:a1e6fb87d648 236 data[0] = (char) MODE1;
aeschsim 55:a1e6fb87d648 237 data[1] = (char) SLEEP;
aeschsim 55:a1e6fb87d648 238 i2c.write((ADRESS<<1), data,2);
aeschsim 55:a1e6fb87d648 239
aeschsim 55:a1e6fb87d648 240 wait_ms(1);
aeschsim 55:a1e6fb87d648 241
aeschsim 55:a1e6fb87d648 242 float prescaleval = 25000000.0; //25MHz
aeschsim 55:a1e6fb87d648 243 prescaleval /= 4096.0f; //12-Bit
aeschsim 42:08b3aea29254 244 prescaleval /= (float)freq;
aeschsim 43:9f291a496db8 245 prescaleval -= 1.0f;
aeschsim 55:a1e6fb87d648 246 char prescale = (char)(prescaleval); //0x64 bei 50Hz
aeschsim 55:a1e6fb87d648 247
aeschsim 55:a1e6fb87d648 248 data[0] = (char) PRESCALE;
aeschsim 55:a1e6fb87d648 249 data[1] = prescale;
aeschsim 55:a1e6fb87d648 250 i2c.write((ADRESS<<1), data,2);
aeschsim 55:a1e6fb87d648 251
aeschsim 55:a1e6fb87d648 252 data[0] = (char) MODE1;
aeschsim 55:a1e6fb87d648 253 data[1] = 0x00; //wake up
aeschsim 55:a1e6fb87d648 254 i2c.write((ADRESS<<1), data,2);
aeschsim 55:a1e6fb87d648 255
aeschsim 55:a1e6fb87d648 256 wait_ms(1);
aeschsim 42:08b3aea29254 257
aeschsim 55:a1e6fb87d648 258 data[0] = (char) MODE1;
aeschsim 55:a1e6fb87d648 259 data[1] = 0x80; //do restart
aeschsim 55:a1e6fb87d648 260 i2c.write((ADRESS<<1), data,2);
aeschsim 55:a1e6fb87d648 261
aeschsim 55:a1e6fb87d648 262 wait_ms(1);
aeschsim 55:a1e6fb87d648 263
aeschsim 55:a1e6fb87d648 264 data[0] = (char) MODE2;
aeschsim 55:a1e6fb87d648 265 data[1] = (char) 0x04;
aeschsim 55:a1e6fb87d648 266 i2c.write((ADRESS<<1), data,2);
aeschsim 55:a1e6fb87d648 267
aeschsim 55:a1e6fb87d648 268 wait_ms(1);
aeschsim 55:a1e6fb87d648 269
aeschsim 55:a1e6fb87d648 270 printf("init done...\r\n");
aeschsim 55:a1e6fb87d648 271 }
aeschsim 42:08b3aea29254 272
aeschsim 55:a1e6fb87d648 273 void set_servo_position(int servo, int deg)
aeschsim 55:a1e6fb87d648 274 {
aeschsim 55:a1e6fb87d648 275 if (deg < 0 || deg > 4095) {
aeschsim 55:a1e6fb87d648 276 deg = 300;
cittecla 34:40d8d29b44b8 277 }
aeschsim 55:a1e6fb87d648 278 char data[2];
aeschsim 55:a1e6fb87d648 279 int16_t wert1 = (deg>>8);
aeschsim 55:a1e6fb87d648 280 int16_t wert2 = (deg & 0xFF);
aeschsim 55:a1e6fb87d648 281 printf("%x %x servo deg\r\n",wert1,wert2);
aeschsim 55:a1e6fb87d648 282
aeschsim 55:a1e6fb87d648 283 data[0] = (char)LED0_ON_L+(4*servo);
aeschsim 55:a1e6fb87d648 284 data[1] = 0x00;
aeschsim 55:a1e6fb87d648 285 i2c.write((ADRESS<<1), data,2);
aeschsim 55:a1e6fb87d648 286
aeschsim 55:a1e6fb87d648 287 data[0] = (char)LED0_ON_H+(4*servo);
aeschsim 55:a1e6fb87d648 288 data[1] = 0x00;
aeschsim 55:a1e6fb87d648 289 i2c.write((ADRESS<<1), data,2);
aeschsim 55:a1e6fb87d648 290
aeschsim 55:a1e6fb87d648 291 data[0] = (char)LED0_OFF_L+(4*servo);
aeschsim 55:a1e6fb87d648 292 data[1] = (char)wert2;
aeschsim 55:a1e6fb87d648 293 i2c.write((ADRESS<<1), data,2);
aeschsim 55:a1e6fb87d648 294
aeschsim 55:a1e6fb87d648 295 data[0] = (char)LED0_OFF_H+(4*servo);
aeschsim 55:a1e6fb87d648 296 data[1] = (char)wert1;
aeschsim 55:a1e6fb87d648 297 i2c.write((ADRESS<<1), data,2);
aeschsim 55:a1e6fb87d648 298 }
aeschsim 55:a1e6fb87d648 299
aeschsim 55:a1e6fb87d648 300 int getPWM(uint8_t servo)
aeschsim 55:a1e6fb87d648 301 {
aeschsim 55:a1e6fb87d648 302 char read = 255;
aeschsim 55:a1e6fb87d648 303 char secondread = 255;
aeschsim 55:a1e6fb87d648 304 char data;
aeschsim 55:a1e6fb87d648 305 data = (char)(LED0_ON_L+(4*servo));
aeschsim 55:a1e6fb87d648 306 i2c.write((ADRESS<<1), &data, 1, 1);
aeschsim 55:a1e6fb87d648 307 i2c.read((ADRESS<<1), &read, 1, 0);
aeschsim 55:a1e6fb87d648 308 data = (char)(LED0_ON_H+(4*servo));
aeschsim 55:a1e6fb87d648 309 i2c.write((ADRESS<<1), &data, 1, 1);
aeschsim 55:a1e6fb87d648 310 i2c.read((ADRESS<<1), &secondread, 1, 0);
aeschsim 55:a1e6fb87d648 311 printf("LED %d ON_L: %x ON_H: %x\r\n",servo, read, secondread);
aeschsim 55:a1e6fb87d648 312 return (int)read;
aeschsim 55:a1e6fb87d648 313 }
aeschsim 55:a1e6fb87d648 314
aeschsim 55:a1e6fb87d648 315 int getPRESCALE(void)
aeschsim 55:a1e6fb87d648 316 {
aeschsim 55:a1e6fb87d648 317 char read = 255;
aeschsim 55:a1e6fb87d648 318 char data;
aeschsim 55:a1e6fb87d648 319 data = (char)(PRESCALE);
aeschsim 55:a1e6fb87d648 320 i2c.write((ADRESS<<1), &data, 1, 1);
aeschsim 55:a1e6fb87d648 321 i2c.read((ADRESS<<1), &read, 1, 0);
aeschsim 55:a1e6fb87d648 322 printf("read Prescale value: >>%x<<\r\n",read);
aeschsim 55:a1e6fb87d648 323 return (int)read;
aeschsim 55:a1e6fb87d648 324 }
cittecla 34:40d8d29b44b8 325
cittecla 34:40d8d29b44b8 326 void enable_servos(){
aeschsim 55:a1e6fb87d648 327 Servo_enable = 0;
cittecla 34:40d8d29b44b8 328 }
cittecla 34:40d8d29b44b8 329
cittecla 34:40d8d29b44b8 330 void disable_servos(){
aeschsim 55:a1e6fb87d648 331 Servo_enable = 1;
cittecla 34:40d8d29b44b8 332 }