Changeset 15 in flair-src for trunk/lib/FlairSensorActuator/src/BlCtrlV2_impl.cpp

Timestamp:

04/08/16 15:40:57 (7 years ago)

Author:

Bayard Gildas

Message:

sources reformatted with flair-format-dir script

File:

• trunk/lib/FlairSensorActuator/src/BlCtrlV2_impl.cpp (modified) (1 diff)

trunk/lib/FlairSensorActuator/src/BlCtrlV2_impl.cpp

@@ -35 +35 @@
 using namespace flair::actuator;
 
-BlCtrlV2_impl::BlCtrlV2_impl(BlCtrlV2* self,Layout *layout,I2cPort* i2cport) {
-    this->self=self;
-        this->i2cport=i2cport;
-        last_voltage_time=0;
-
-    DetectMotors();
-    //if(nb_mot!=self->MotorsCount()) self->Err("motors count different from multiplex count\n");
-
-    //BatteryMonitor
-    battery=new BatteryMonitor(layout->NewRow(),"battery");
-
-    //user interface
-    GroupBox *setupgroupbox=new GroupBox(layout->NewRow(),"Motors");
-    poles=new SpinBox(setupgroupbox->NewRow(),"nb poles:",0,255,1,14);
-}
-
-BlCtrlV2_impl::~BlCtrlV2_impl() {
-
-}
-
-void BlCtrlV2_impl::SetMotors(float* value) {
-    uint16_t tosend_value[nb_mot];
-    //stocke dans une variable pour garder le moins longtemps l'i2c (pour ne pas bloquer sur le mutex de l'output)
-    float speeds[nb_mot];
-    float currents[nb_mot];
-
-    for(int i=0;i<nb_mot;i++) tosend_value[i]=(uint16_t)(MAX_VALUE*value[i]);
-
-    i2cport->GetMutex();
-
-    for(int i=0;i<nb_mot;i++) {
-        i2cport->SetSlave(BASE_ADDRESS+i);
-        WriteValue(tosend_value[i]);
+BlCtrlV2_impl::BlCtrlV2_impl(BlCtrlV2 *self, Layout *layout, I2cPort *i2cport) {
+  this->self = self;
+  this->i2cport = i2cport;
+  last_voltage_time = 0;
+
+  DetectMotors();
+  // if(nb_mot!=self->MotorsCount()) self->Err("motors count different from
+  // multiplex count\n");
+
+  // BatteryMonitor
+  battery = new BatteryMonitor(layout->NewRow(), "battery");
+
+  // user interface
+  GroupBox *setupgroupbox = new GroupBox(layout->NewRow(), "Motors");
+  poles = new SpinBox(setupgroupbox->NewRow(), "nb poles:", 0, 255, 1, 14);
+}
+
+BlCtrlV2_impl::~BlCtrlV2_impl() {}
+
+void BlCtrlV2_impl::SetMotors(float *value) {
+  uint16_t tosend_value[nb_mot];
+  // stocke dans une variable pour garder le moins longtemps l'i2c (pour ne pas
+  // bloquer sur le mutex de l'output)
+  float speeds[nb_mot];
+  float currents[nb_mot];
+
+  for (int i = 0; i < nb_mot; i++)
+    tosend_value[i] = (uint16_t)(MAX_VALUE * value[i]);
+
+  i2cport->GetMutex();
+
+  for (int i = 0; i < nb_mot; i++) {
+    i2cport->SetSlave(BASE_ADDRESS + i);
+    WriteValue(tosend_value[i]);
+  }
+
+  for (int i = 0; i < nb_mot; i++) {
+    i2cport->SetSlave(BASE_ADDRESS + i);
+
+    if (i == 0 && GetTime() > (last_voltage_time + 5 * (Time)1000000000)) {
+      // toute les 5 secondes sur moteur 0
+      float voltage;
+      GetCurrentSpeedAndVoltage(currents[i], speeds[i], voltage);
+      battery->SetBatteryValue(voltage);
+      last_voltage_time = GetTime();
+    } else {
+      GetCurrentAndSpeed(currents[i], speeds[i]);
     }
-
-    for(int i=0;i<nb_mot;i++) {
-        i2cport->SetSlave(BASE_ADDRESS+i);
-
-        if(i==0 && GetTime()>(last_voltage_time+5*(Time)1000000000)) {
-            //toute les 5 secondes sur moteur 0
-            float voltage;
-            GetCurrentSpeedAndVoltage(currents[i],speeds[i],voltage);
-            battery->SetBatteryValue(voltage);
-            last_voltage_time=GetTime();
-        } else {
-            GetCurrentAndSpeed(currents[i],speeds[i]);
-        }
+  }
+  // printf("%f %f %f %f\n",speeds[0],speeds[1],speeds[2],speeds[3]);
+  /*
+      if(GetTime()>(last_voltage_time+5*(Time)1000000000))//toute les 5 secondes
+      {
+          i2cport->SetSlave(BASE_ADDRESS);
+          battery->SetBatteryValue(ReadVoltage());
+          last_voltage_time=GetTime();
+      }
+  */
+  i2cport->ReleaseMutex();
+
+  // on prend une fois pour toute le mutex et on fait des accès directs
+  cvmatrix *output = self->output;
+  output->GetMutex();
+  for (int i = 0; i < nb_mot; i++)
+    output->SetValueNoMutex(i, 0, speeds[i]);
+  for (int i = 0; i < nb_mot; i++)
+    output->SetValueNoMutex(i, 1, currents[i]);
+  output->ReleaseMutex();
+}
+
+// I2cPort mutex must be taken before calling this function
+void BlCtrlV2_impl::WriteValue(uint16_t value) {
+  uint8_t tx[2];
+  ssize_t written;
+
+  tx[0] = (uint8_t)(value >> 3); // msb
+  tx[1] = (value & 0x07);
+  ; //+16+8; //pour recuperer la vitesse en premier
+  written = i2cport->Write(tx, 2);
+  if (written < 0) {
+    self->Err("rt_dev_write error (%s)\n", strerror(-written));
+  } else if (written != sizeof(tx)) {
+    self->Err("rt_dev_write error %i/%i\n", written, sizeof(tx));
+  }
+}
+
+// I2cPort mutex must be taken before calling this function
+void BlCtrlV2_impl::GetCurrentAndSpeed(float &current, float &speed) {
+  ssize_t read;
+  uint8_t value[4];
+  read = i2cport->Read(value, sizeof(value));
+
+  if (read < 0) {
+    self->Err("rt_dev_read error (%s)\n", strerror(-read));
+    speed = -1;
+    current = -1;
+  } else if (read != sizeof(value)) {
+    self->Err("rt_dev_read error %i/%i\n", read, sizeof(value));
+    speed = -1;
+    current = -1;
+  } else {
+    current = value[0] / 10.;
+    speed = value[3] * 780. / poles->Value();
+  }
+}
+
+// I2cPort mutex must be taken before calling this function
+void BlCtrlV2_impl::GetCurrentSpeedAndVoltage(float &current, float &speed,
+                                              float &voltage) {
+  ssize_t read;
+  uint8_t value[6];
+  read = i2cport->Read(value, sizeof(value));
+
+  if (read < 0) {
+    self->Err("rt_dev_read error (%s)\n", strerror(-read));
+    speed = -1;
+    voltage = -1;
+    current = -1;
+  } else if (read != sizeof(value)) {
+    self->Err("rt_dev_read error %i/%i\n", read, sizeof(value));
+    speed = -1;
+    voltage = -1;
+    current = -1;
+  } else {
+    current = value[0] / 10.;
+    voltage = value[5] / 10.;
+    speed = value[3] * 780. / poles->Value();
+  }
+}
+
+void BlCtrlV2_impl::DetectMotors(void) {
+  int nb = 0;
+  ssize_t read, nb_write;
+  uint8_t value[3];
+  uint8_t tx[2];
+
+  i2cport->GetMutex();
+
+  for (int i = 0; i < MAX_MOTORS; i++) {
+    // printf("test %i\n",i);
+    i2cport->SetSlave(BASE_ADDRESS + i);
+    tx[0] = 0;
+    tx[1] = 16 + 8; // 16+8 pour recuperer la vitesse
+
+    nb_write = i2cport->Write(tx, 2);
+
+    if (nb_write != sizeof(tx)) {
+      continue;
     }
-//printf("%f %f %f %f\n",speeds[0],speeds[1],speeds[2],speeds[3]);
-/*
-    if(GetTime()>(last_voltage_time+5*(Time)1000000000))//toute les 5 secondes
-    {
-        i2cport->SetSlave(BASE_ADDRESS);
-        battery->SetBatteryValue(ReadVoltage());
-        last_voltage_time=GetTime();
+    nb++;
+  }
+
+  for (int i = 0; i < MAX_MOTORS; i++) {
+    i2cport->SetSlave(BASE_ADDRESS + i);
+    read = i2cport->Read(value, 3);
+
+    if (read != sizeof(value)) {
+      continue;
     }
-*/
-    i2cport->ReleaseMutex();
-
-    //on prend une fois pour toute le mutex et on fait des accès directs
-    cvmatrix* output=self->output;
-    output->GetMutex();
-    for(int i=0;i<nb_mot;i++) output->SetValueNoMutex(i,0,speeds[i]);
-    for(int i=0;i<nb_mot;i++) output->SetValueNoMutex(i,1,currents[i]);
-    output->ReleaseMutex();
-}
-
-//I2cPort mutex must be taken before calling this function
-void BlCtrlV2_impl::WriteValue(uint16_t value) {
-    uint8_t tx[2];
-    ssize_t written;
-
-    tx[0]=(uint8_t)(value>>3);//msb
-    tx[1]=(value&0x07);;//+16+8; //pour recuperer la vitesse en premier
-    written =i2cport->Write(tx, 2);
-    if(written<0) {
-        self->Err("rt_dev_write error (%s)\n",strerror(-written));
-    } else if (written != sizeof(tx)) {
-        self->Err("rt_dev_write error %i/%i\n",written,sizeof(tx));
-    }
-}
-
-//I2cPort mutex must be taken before calling this function
-void BlCtrlV2_impl::GetCurrentAndSpeed(float &current,float &speed) {
-    ssize_t read;
-    uint8_t value[4];
-    read = i2cport->Read(value, sizeof(value));
-
-    if(read<0) {
-        self->Err("rt_dev_read error (%s)\n",strerror(-read));
-        speed=-1;
-        current=-1;
-    } else if (read != sizeof(value)) {
-        self->Err("rt_dev_read error %i/%i\n",read,sizeof(value));
-        speed=-1;
-        current=-1;
-    } else {
-        current=value[0]/10.;
-        speed= value[3]*780./poles->Value();
-    }
-}
-
-//I2cPort mutex must be taken before calling this function
-void BlCtrlV2_impl::GetCurrentSpeedAndVoltage(float &current,float &speed,float &voltage) {
-    ssize_t read;
-    uint8_t value[6];
-    read = i2cport->Read(value, sizeof(value));
-
-    if(read<0) {
-        self->Err("rt_dev_read error (%s)\n",strerror(-read));
-        speed=-1;
-        voltage=-1;
-        current=-1;
-    } else if (read != sizeof(value)) {
-        self->Err("rt_dev_read error %i/%i\n",read,sizeof(value));
-        speed=-1;
-        voltage=-1;
-        current=-1;
-    } else {
-        current=value[0]/10.;
-        voltage=value[5]/10.;
-        speed= value[3]*780./poles->Value();
-    }
-}
-
-void BlCtrlV2_impl::DetectMotors(void) {
-    int nb=0;
-    ssize_t read,nb_write;
-    uint8_t value[3];
-    uint8_t tx[2];
-
-    i2cport->GetMutex();
-
-    for(int i=0;i<MAX_MOTORS;i++) {
-        //printf("test %i\n",i);
-        i2cport->SetSlave(BASE_ADDRESS+i);
-        tx[0]=0;
-        tx[1]=16+8;//16+8 pour recuperer la vitesse
-
-        nb_write = i2cport->Write(tx, 2);
-
-        if (nb_write != sizeof(tx)) {
-            continue;
-        }
-        nb++;
-    }
-
-    for(int i=0;i<MAX_MOTORS;i++) {
-        i2cport->SetSlave(BASE_ADDRESS+i);
-        read = i2cport->Read(value, 3);
-
-        if (read != sizeof(value)) {
-            continue;
-        }
-    }
-
-    i2cport->ReleaseMutex();
-
-    Printf("BlCtrlV2: Dectected motors: %i\n",nb);
-    if(nb==4) {
-        Printf("BlCtrlV2: Configuration X4\n");
-    } else if(nb==8) {
-        Printf("BlCtrlV2: Configuration X8\n");
-    } else {
-        //self->Err("Error, configuration not supported (%i/%i)\n",nb,MAX_MOTORS);
-    }
-
-    nb_mot=nb;
-}
+  }
+
+  i2cport->ReleaseMutex();
+
+  Printf("BlCtrlV2: Dectected motors: %i\n", nb);
+  if (nb == 4) {
+    Printf("BlCtrlV2: Configuration X4\n");
+  } else if (nb == 8) {
+    Printf("BlCtrlV2: Configuration X8\n");
+  } else {
+    // self->Err("Error, configuration not supported (%i/%i)\n",nb,MAX_MOTORS);
+  }
+
+  nb_mot = nb;
+}