Diff: source/Robot.cpp

Revision:

43:9f291a496db8

Parent:

42:08b3aea29254

Child:

55:a1e6fb87d648

Child:

57:1a395b6928ee

--- a/source/Robot.cpp	Tue Apr 11 13:34:36 2017 +0000
+++ b/source/Robot.cpp	Tue Apr 11 14:12:25 2017 +0000
@@ -19,7 +19,7 @@
 const float MIN_DUTY_CYCLE = 0.02f;             // minimum allowed value for duty cycle (2%)
 const float MAX_DUTY_CYCLE = 0.98f;             // maximum allowed value for duty cycle (98%)
 const float correction_value = 2.45f;           // correction value for desired speed
-
+    
 LowpassFilter speedLeftFilter;
 LowpassFilter speedRightFilter;
 
@@ -57,6 +57,17 @@
 I2C i2c(PB_9, PB_8);
 DigitalOut Servo_enable(PA_10);
 
+uint8_t mode1 = 0x00;
+uint8_t mode2 = 0x01;
+uint8_t LED0_ON_L          = 0x06;
+uint8_t LED0_ON_H          = 0x07;
+uint8_t LED0_OFF_L         = 0x08;
+uint8_t LED0_OFF_H         = 0x09;
+uint8_t ALL_LED_ON_L       = 0xFA;
+uint8_t ALL_LED_ON_H       = 0xFB;
+uint8_t ALL_LED_OFF_L      = 0xFC;
+uint8_t ALL_LED_OFF_H      = 0xFD;
+
 
 short previousValueCounterRight = 0;
 short previousValueCounterLeft = 0;
@@ -194,29 +205,33 @@
 
 
 // Servo I2C
-void init_servo(int freq) {
-    uint8_t mode1 = 0x00;
-    uint8_t mode2 = 0x01;
-     
+void init_servo(int freq) { 
     i2c.write(0x06); //0000 0110 - PCA reset
     //freq *= 0.9;
     float prescaleval = 25000000.0;  //25MHz
-    prescaleval /= 4096.0;           //12-Bit
+    prescaleval /= 4096.0f;           //12-Bit
     prescaleval /= (float)freq;
-    prescaleval -= 1.0;
-    uint8_t prescale = floor(prescaleval +0.5);
+    prescaleval -= 1.0f;
+    uint8_t prescale = floor(prescaleval +0.5f);
     
     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);
+    wait_ms(50);
     i2c.write((mode1<<8) + (oldmode|0xA1));
     }
 
 void set_servo_position(int servo, int deg){
-    i2c.write((0x40), &data, 2);
+    uint16_t on = 0x00; 
+    uint16_t off;
+    off = 150 + (3 * deg);  //deg von 0...150° abgebildet in min150...max600
+    i2c.write(LED0_ON_L+4*servo);
+    i2c.write(on);
+    i2c.write(on>>8);
+    i2c.write(off);
+    i2c.write(off>>8);
     }
     
 void enable_servos(){