fix demo02

Author: gbr1

examples/demo_02/demo_02.ino

@@ -42,9 +42,9 @@ void simpleMotors(){
       case 0:
         robot.motor_left->setSpeed(4095);
         robot.motor_right->setSpeed(4095);
-        robot.led1->set(HIGH,LOW,LOW);
-        robot.led2->set(LOW,LOW,HIGH);
-        if (millis()-tmotor>2000){
+        robot.setLedLeft(COLOR_BLUE);
+        robot.setLedRight(COLOR_RED);
+        if (millis()-tmotor>1000){
           status++;
           tmotor=millis();
           serial.println("forward");
@@ -53,8 +53,8 @@ void simpleMotors(){
       case 1:
         robot.motor_left->setSpeed(4095);
         robot.motor_right->setSpeed(-4095);
-        robot.led1->set(LOW,LOW,HIGH);
-        robot.led2->set(HIGH,LOW,LOW);
+        robot.setLedLeft(COLOR_RED);
+        robot.setLedRight(COLOR_BLUE);
         if (millis()-tmotor>1000){
           status++;
           status=0;
@@ -67,8 +67,7 @@ void simpleMotors(){
   }
   robot.motor_left->setSpeed(0);
   robot.motor_right->setSpeed(0);
-  robot.led1->set(LOW,LOW,LOW);
-  robot.led2->set(LOW,LOW,LOW);
+  robot.setLeds(COLOR_BLACK);
 }
 
 void line_follower(){
@@ -89,17 +88,16 @@ void line_follower(){
       serial.println(control);
 
       if ((control<1)&&(control>-1)){
-        robot.led1->set(LOW,HIGH,LOW);
-        robot.led2->set(LOW,HIGH,LOW);
+        robot.setLeds(COLOR_GREEN);
       }
       else{
         if (control>=1){
-          robot.led1->set(HIGH,HIGH,LOW);
-          robot.led2->set(LOW,LOW,LOW);
+            robot.setLedLeft(COLOR_ORANGE);
+            robot.setLedRight(COLOR_BLACK);
         }
         else{
-          robot.led1->set(LOW,LOW,LOW);
-          robot.led2->set(HIGH,HIGH,LOW);
+            robot.setLedLeft(COLOR_BLACK);
+            robot.setLedRight(COLOR_ORANGE);
         }
       }
 
@@ -128,6 +126,5 @@ void line_follower(){
   }
   robot.motor_left->setSpeed(0);
   robot.motor_right->setSpeed(0);
-  robot.led1->set(LOW,LOW,LOW);
-  robot.led2->set(LOW,LOW,LOW);
+  robot.setLeds(COLOR_BLACK);
 }

src/Arduino_Robot_Firmware.cpp

@@ -8,7 +8,7 @@ Arduino_Robot_Firmware::Arduino_Robot_Firmware(){
     wire = new TwoWire(I2C_1_SDA, I2C_1_SCL);
 
     // I2C external bus
-    //ext_wire = new TwoWire(I2C_2_SDA,I2C_2_SCL);
+    ext_wire = new TwoWire(I2C_2_SDA,I2C_2_SCL);
 
     // RGB leds
     led1 = new RGBled(LED_1_RED,LED_1_GREEN,LED_1_BLUE);
@@ -63,16 +63,16 @@ int Arduino_Robot_Firmware::begin(){
 
     wire->begin();
     wire->setClock(400000);
-
-    //ext_wire->begin(ARDUINO_ROBOT_ADDRESS);
+    
+    connectExternalI2C();
+    ext_wire->begin(ARDUINO_ROBOT_ADDRESS);
 
     //beginImu();
 
     if (beginAPDS()!=0){
         errorLed(ERROR_APDS);
     }
-    
-    
+     
     beginServo();
     //beginI2Cselect();
     //connectExternalI2C();
@@ -81,7 +81,6 @@ int Arduino_Robot_Firmware::begin(){
         errorLed(ERROR_BMS);
     }
 
-
     if (beginTouch()!=0){
         //errorLed(ERROR_TOUCH);
     }

src/Arduino_Robot_Firmware.h

@@ -45,7 +45,7 @@ class Arduino_Robot_Firmware{
 
 
         TwoWire * wire;
-        //TwoWire * ext_wire;
+        TwoWire * ext_wire;