Diff: source/Robot.cpp

Revision:

58:a78103b0bf2a

Parent:

57:1a395b6928ee

Parent:

55:a1e6fb87d648

Child:

60:b57577b0072f

--- a/source/Robot.cpp	Tue Apr 18 15:38:15 2017 +0000
+++ b/source/Robot.cpp	Tue Apr 18 15:39:45 2017 +0000
@@ -9,6 +9,18 @@
 
 #include "IRSensor.h"
 
+//Define I2C-Servoboard Consts
+#define ADRESS          0x40
+#define MODE1           0x00
+#define MODE2           0x01
+#define PRESCALE        0xFE
+#define LED0_ON_L       0x06
+#define LED0_ON_H       0x07
+#define LED0_OFF_L      0x08
+
+#define RESTART         0x80
+#define SLEEP           0x10
+
 
 const float PERIOD = 0.001f;                    // period of control task, given in [s]
 const float COUNTS_PER_TURN = 1200.0f;          // resolution of encoder counter
@@ -23,6 +35,8 @@
 LowpassFilter speedLeftFilter;
 LowpassFilter speedRightFilter;
 
+DigitalIn user(USER_BUTTON);
+
 //Motor stuff
 EncoderCounter counterLeft(PB_6, PB_7);
 EncoderCounter counterRight(PA_6, PC_7);
@@ -35,6 +49,7 @@
 
 //Periphery for distance sensors
 AnalogIn distance(PB_1);
+AnalogIn distance2(PA_1);
 DigitalOut enable(PC_1);
 DigitalOut bit0(PH_1);
 DigitalOut bit1(PC_2);
@@ -57,17 +72,9 @@
 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;
-
+//Color sensor
+DigitalIn Red(PB_13);
+DigitalIn Green(PC_4);
 
 short previousValueCounterRight = 0;
 short previousValueCounterLeft = 0;
@@ -79,6 +86,10 @@
 float actualSpeedRight;
 
 
+bool get_user() 
+{
+    return user;
+}    
 
 
 void Robot_init_all()
@@ -167,6 +178,15 @@
     return sensors[number];
 }
 
+//Color
+bool get_color() {
+    if (red == 0 && green == 1) {
+        return 1;
+    } else {
+        return 0;
+    }
+}
+
 //IMU
 float read_acc_x()
 {
@@ -206,39 +226,107 @@
 
 
 // Servo I2C
-void init_servo(int freq) { 
-    i2c.write(0x06); //0000 0110 - PCA reset
-    //freq *= 0.9;
-    float prescaleval = 25000000.0;  //25MHz
-    prescaleval /= 4096.0f;           //12-Bit
+
+
+void init_servo(int freq)
+{
+    char data[2];
+
+    //Reset
+    data[0] = (char) MODE1;
+    data[1] = (char) SLEEP;  
+    i2c.write((ADRESS<<1), data,2);
+    
+    wait_ms(1);
+    
+    float prescaleval = 25000000.0;     //25MHz
+    prescaleval /= 4096.0f;             //12-Bit
     prescaleval /= (float)freq;
     prescaleval -= 1.0f;
-    uint8_t prescale = floor(prescaleval +0.5f);
+    char prescale = (char)(prescaleval);    //0x64 bei 50Hz
+    
+    data[0] = (char) PRESCALE;
+    data[1] = prescale;
+    i2c.write((ADRESS<<1), data,2);
+    
+    data[0] = (char) MODE1;
+    data[1] = 0x00;             //wake up
+    i2c.write((ADRESS<<1), data,2);
+    
+    wait_ms(1);
     
-    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);
-    wait_ms(50);
-    i2c.write((mode1<<8) + (oldmode|0xA1));
-    }
+    data[0] = (char) MODE1;
+    data[1] = 0x80;             //do restart 
+    i2c.write((ADRESS<<1), data,2);
+    
+    wait_ms(1);
+    
+    data[0] = (char) MODE2;
+    data[1] = (char) 0x04;
+    i2c.write((ADRESS<<1), data,2);
+    
+    wait_ms(1);
+
+    printf("init done...\r\n");
+}
 
-void set_servo_position(int servo, int deg){
-    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 set_servo_position(int servo, int deg)
+{
+    if (deg < 0 || deg > 4095) {
+        deg = 300;
     }
+    char data[2];
+    int16_t wert1 = (deg>>8);
+    int16_t wert2 = (deg & 0xFF);
+    printf("%x %x servo deg\r\n",wert1,wert2);
+    
+    data[0] = (char)LED0_ON_L+(4*servo);
+    data[1] = 0x00;
+    i2c.write((ADRESS<<1), data,2);
+    
+    data[0] = (char)LED0_ON_H+(4*servo);
+    data[1] = 0x00;
+    i2c.write((ADRESS<<1), data,2);
+    
+    data[0] = (char)LED0_OFF_L+(4*servo);
+    data[1] = (char)wert2;
+    i2c.write((ADRESS<<1), data,2);
+    
+    data[0] = (char)LED0_OFF_H+(4*servo);
+    data[1] = (char)wert1;
+    i2c.write((ADRESS<<1), data,2);   
+}
+
+int getPWM(uint8_t servo) 
+{
+    char read = 255;
+    char secondread = 255;
+    char data;
+    data = (char)(LED0_ON_L+(4*servo));
+    i2c.write((ADRESS<<1), &data, 1, 1);
+    i2c.read((ADRESS<<1), &read, 1, 0);
+    data = (char)(LED0_ON_H+(4*servo));
+    i2c.write((ADRESS<<1), &data, 1, 1);
+    i2c.read((ADRESS<<1), &secondread, 1, 0);
+    printf("LED %d ON_L: %x ON_H: %x\r\n",servo, read, secondread);
+    return (int)read;
+}
+
+int getPRESCALE(void) 
+{
+    char read = 255;
+    char data;
+    data = (char)(PRESCALE);
+    i2c.write((ADRESS<<1), &data, 1, 1);
+    i2c.read((ADRESS<<1), &read, 1, 0);
+    printf("read Prescale value: >>%x<<\r\n",read);
+    return (int)read;
+}
     
 void enable_servos(){
-    Servo_enable = 1;
+    Servo_enable = 0;
     }
     
 void disable_servos(){
-    Servo_enable = 0;
+    Servo_enable = 1;
     }