Merge branch 'devel' of https://github.com/arduino-libraries/Arduino_AlvikCarrier into devel

Author: eigen-value

examples/firmware_01/firmware_01.ino

@@ -231,7 +231,7 @@ void loop(){
         break;
       case 3:
         if (tof.update_rois()){
-          msg_size = packeter.packetC7I('f', tof.getLeft(), tof.getCenterLeft(), tof.getCenter(), tof.getCenterRight(), tof.getRight(), tof.get_min_range_top_mm(), tof.get_max_range_bottom_mm());
+          msg_size = packeter.packetC7I('f', tof.getLeft(), tof.getCenterLeft(), tof.getCenter(), tof.getCenterRight(), tof.getRight(), tof.getTop(), tof.getBottom());
           alvik.serial->write(packeter.msg,msg_size);
         }
         break;
@@ -247,7 +247,7 @@ void loop(){
   } 
 
   // motors update & publish
-  if (millis()-tmotor>20){
+  if (millis()-tmotor>=20){
     tmotor=millis();
     alvik.updateMotors();
     alvik.updateKinematics();

src/motor_control/motor_control.cpp

@@ -42,6 +42,8 @@ MotorControl::MotorControl(DCmotor * _motor, Encoder * _encoder, const float _kp
     angle = 0.0;
     reference = 0.0;
 
+    return_flag = false;
+
     trip = 0.0;
     iterations = 0.0;
     start_value = 0.0;
@@ -111,22 +113,28 @@ float MotorControl::getRPM(){
 
 
 bool MotorControl::setRPM(const float ref){
-    if ((ref<=MOTOR_LIMIT)&&(ref>=-MOTOR_LIMIT)){
-        reference = ref;
-        if (control_mode==CONTROL_MODE_LINEAR){
-            start_value=interpolation;
-            end_value=reference;
-            trip=0.0;
-            iterations=abs(end_value-start_value)/step_size;
-            step=1.0/iterations;
-            step_index=0;
-        }
-        else if(control_mode==CONTROL_MODE_NORMAL){
-            vel_pid->setReference(reference);
-        }
-        return true;
+    reference = ref;
+    return_flag = true;
+    if (ref>MOTOR_LIMIT){
+        reference=MOTOR_LIMIT;
+        return_flag = false;
+    }
+    if (ref<-MOTOR_LIMIT){
+        reference=-MOTOR_LIMIT;
+        return_flag = false;
+    }
+    if (control_mode==CONTROL_MODE_LINEAR){
+        start_value=interpolation;
+        end_value=reference;
+        trip=0.0;
+        iterations=abs(end_value-start_value)/step_size;
+        step=1.0/iterations;
+        step_index=0;
+    }
+    else if(control_mode==CONTROL_MODE_NORMAL){
+        vel_pid->setReference(reference);
     }
-    return false;
+    return return_flag;
 }
 
 void MotorControl::update(){

src/motor_control/motor_control.h

@@ -29,6 +29,8 @@ class MotorControl{
     private:
 
         uint8_t control_mode;
+        bool return_flag;
+
 
         float kp;
         float ki;

src/sensor_tof_matrix.h

@@ -133,7 +133,7 @@ class SensorTofMatrix{
         bool update_rois() {
             bool out = update();
             top = get_avg_range_top_mm();
-            bottom = get_max_range_bottom_mm();
+            bottom = get_avg_range_bottom_mm();
             left = get_avg_range_left_mm();
             right = get_avg_range_right_mm();
             center_left = get_avg_range_center_left_mm();
@@ -168,7 +168,7 @@ class SensorTofMatrix{
 
         int get_max_range_bottom_mm() {
 
-            int16_t bottom_max = results.distance_mm[0];
+            int16_t bottom_max = results.distance_mm[4?12:48];
 
             for (int i=(_size==4?12:48); i < (_size==4?15:63) ;i++) {
                 bottom_max = max(bottom_max, results.distance_mm[i]);
@@ -206,7 +206,7 @@ class SensorTofMatrix{
 
         int get_min_range_right_mm() {
 
-            int16_t _min = results.distance_mm[0];
+            int16_t _min = results.distance_mm[4?3:6];
 
             for (int i=(_size==4?3:6); i < (_size==4?16:64) ;i+=_size) {
                 _min = min(_min, results.distance_mm[i]);
@@ -220,7 +220,7 @@ class SensorTofMatrix{
 
         int get_min_range_center_left_mm() {
 
-            int16_t _min = results.distance_mm[0];
+            int16_t _min = results.distance_mm[4?5:18];
 
             for (int i=(_size==4?5:18); i < (_size==4?13:50) ;i+=_size) {
                 _min = min(_min, results.distance_mm[i]);
@@ -234,7 +234,7 @@ class SensorTofMatrix{
 
         int get_min_range_center_right_mm() {
 
-            int16_t _min = results.distance_mm[0];
+            int16_t _min = results.distance_mm[4?6:20];
 
             for (int i=(_size==4?6:20); i < (_size==4?14:52) ;i+=_size) {
                 _min = min(_min, results.distance_mm[i]);
@@ -248,7 +248,7 @@ class SensorTofMatrix{
 
         int get_min_range_center_mm() {
 
-            int16_t _min = results.distance_mm[0];
+            int16_t _min = results.distance_mm[5?6:19];
 
             for (int i=(_size==5?6:19); i < (_size==4?13:51) ;i+=_size) {
                 _min = min(_min, results.distance_mm[i]);