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@94:0381e8b1beda, 2017-05-02 (annotated)

Committer:
cittecla
Date:
Tue May 02 15:29:50 2017 +0000
Revision:
94:0381e8b1beda
Parent:
93:837a13760026
Child:
95:5afdfe17300f
changed something in move for brick

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