Diff: source/Robot.cpp

Revision:

61:628f8a4e857c

Parent:

60:b57577b0072f

Child:

62:c2fcf3b349e9

diff -r b57577b0072f -r 628f8a4e857c source/Robot.cpp
--- a/source/Robot.cpp	Tue Apr 18 16:19:18 2017 +0000
+++ b/source/Robot.cpp	Tue Apr 18 17:33:22 2017 +0000
@@ -32,7 +32,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;
 
@@ -87,10 +87,10 @@
 float actualSpeedRight;
 
 
-bool get_user() 
+bool get_user()
 {
     return user;
-}    
+}
 
 
 void Robot_init_all()
@@ -136,6 +136,16 @@
     desiredSpeedRight = -right; //50 RPM
 }
 
+float get_speed_left()
+{
+    return actualSpeedLeft;
+}
+
+float get_speed_right()
+{
+    return actualSpeedRight;
+}
+
 
 void speedCtrl()
 {
@@ -180,7 +190,8 @@
 }
 
 //Color
-bool get_color() {
+bool get_color()
+{
     if (red == 0 && green == 1) {
         return 1;
     } else {
@@ -235,37 +246,37 @@
 
     //Reset
     data[0] = (char) MODE1;
-    data[1] = (char) SLEEP;  
+    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;
     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);
-    
+
     data[0] = (char) MODE1;
-    data[1] = 0x80;             //do restart 
+    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");
@@ -280,25 +291,25 @@
     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);   
+    i2c.write((ADRESS<<1), data,2);
 }
 
-int getPWM(uint8_t servo) 
+int getPWM(uint8_t servo)
 {
     char read = 255;
     char secondread = 255;
@@ -313,7 +324,7 @@
     return (int)read;
 }
 
-int getPRESCALE(void) 
+int getPRESCALE(void)
 {
     char read = 255;
     char data;
@@ -323,11 +334,13 @@
     printf("read Prescale value: >>%x<<\r\n",read);
     return (int)read;
 }
-    
-void enable_servos(){
+
+void enable_servos()
+{
     Servo_enable = 0;
-    }
-    
-void disable_servos(){
+}
+
+void disable_servos()
+{
     Servo_enable = 1;
-    }
+}