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

Diff: source/Robot.cpp

Revision:
42:08b3aea29254
Parent:
38:3526c36e4c73
Child:
43:9f291a496db8
--- a/source/Robot.cpp	Mon Apr 10 14:52:23 2017 +0000
+++ b/source/Robot.cpp	Tue Apr 11 13:34:36 2017 +0000
@@ -194,7 +194,28 @@
 
 
 // Servo I2C
-void set_servo_position(int servo, char data){
+void init_servo(int freq) {
+    uint8_t mode1 = 0x00;
+    uint8_t mode2 = 0x01;
+     
+    i2c.write(0x06); //0000 0110 - PCA reset
+    //freq *= 0.9;
+    float prescaleval = 25000000.0;  //25MHz
+    prescaleval /= 4096.0;           //12-Bit
+    prescaleval /= (float)freq;
+    prescaleval -= 1.0;
+    uint8_t prescale = floor(prescaleval +0.5);
+    
+    uint8_t oldmode = i2c.read(mode1);
+    uint8_t newmode = (oldmode&0x7f) | 0x10;
+    i2c.write(mode1<<8 + newmode);
+    i2c.write(0xFE<<8 + prescale);   //0xFE - Prescale-Register
+    i2c.write(mode1<<8 + oldmode);
+    delay(5);
+    i2c.write((mode1<<8) + (oldmode|0xA1));
+    }
+
+void set_servo_position(int servo, int deg){
     i2c.write((0x40), &data, 2);
     }