Ignore:
Timestamp:
Apr 8, 2016, 3:40:57 PM (6 years ago)
Author:
Bayard Gildas
Message:

sources reformatted with flair-format-dir script

File:
1 edited

Legend:

Unmodified
Added
Removed
  • trunk/lib/FlairSensorActuator/src/BlCtrlV2_impl.cpp

    r3 r15  
    3535using namespace flair::actuator;
    3636
    37 BlCtrlV2_impl::BlCtrlV2_impl(BlCtrlV2* self,Layout *layout,I2cPort* i2cport) {
    38     this->self=self;
    39         this->i2cport=i2cport;
    40         last_voltage_time=0;
    41 
    42     DetectMotors();
    43     //if(nb_mot!=self->MotorsCount()) self->Err("motors count different from multiplex count\n");
    44 
    45     //BatteryMonitor
    46     battery=new BatteryMonitor(layout->NewRow(),"battery");
    47 
    48     //user interface
    49     GroupBox *setupgroupbox=new GroupBox(layout->NewRow(),"Motors");
    50     poles=new SpinBox(setupgroupbox->NewRow(),"nb poles:",0,255,1,14);
    51 }
    52 
    53 BlCtrlV2_impl::~BlCtrlV2_impl() {
    54 
    55 }
    56 
    57 void BlCtrlV2_impl::SetMotors(float* value) {
    58     uint16_t tosend_value[nb_mot];
    59     //stocke dans une variable pour garder le moins longtemps l'i2c (pour ne pas bloquer sur le mutex de l'output)
    60     float speeds[nb_mot];
    61     float currents[nb_mot];
    62 
    63     for(int i=0;i<nb_mot;i++) tosend_value[i]=(uint16_t)(MAX_VALUE*value[i]);
    64 
    65     i2cport->GetMutex();
    66 
    67     for(int i=0;i<nb_mot;i++) {
    68         i2cport->SetSlave(BASE_ADDRESS+i);
    69         WriteValue(tosend_value[i]);
     37BlCtrlV2_impl::BlCtrlV2_impl(BlCtrlV2 *self, Layout *layout, I2cPort *i2cport) {
     38  this->self = self;
     39  this->i2cport = i2cport;
     40  last_voltage_time = 0;
     41
     42  DetectMotors();
     43  // if(nb_mot!=self->MotorsCount()) self->Err("motors count different from
     44  // multiplex count\n");
     45
     46  // BatteryMonitor
     47  battery = new BatteryMonitor(layout->NewRow(), "battery");
     48
     49  // user interface
     50  GroupBox *setupgroupbox = new GroupBox(layout->NewRow(), "Motors");
     51  poles = new SpinBox(setupgroupbox->NewRow(), "nb poles:", 0, 255, 1, 14);
     52}
     53
     54BlCtrlV2_impl::~BlCtrlV2_impl() {}
     55
     56void BlCtrlV2_impl::SetMotors(float *value) {
     57  uint16_t tosend_value[nb_mot];
     58  // stocke dans une variable pour garder le moins longtemps l'i2c (pour ne pas
     59  // bloquer sur le mutex de l'output)
     60  float speeds[nb_mot];
     61  float currents[nb_mot];
     62
     63  for (int i = 0; i < nb_mot; i++)
     64    tosend_value[i] = (uint16_t)(MAX_VALUE * value[i]);
     65
     66  i2cport->GetMutex();
     67
     68  for (int i = 0; i < nb_mot; i++) {
     69    i2cport->SetSlave(BASE_ADDRESS + i);
     70    WriteValue(tosend_value[i]);
     71  }
     72
     73  for (int i = 0; i < nb_mot; i++) {
     74    i2cport->SetSlave(BASE_ADDRESS + i);
     75
     76    if (i == 0 && GetTime() > (last_voltage_time + 5 * (Time)1000000000)) {
     77      // toute les 5 secondes sur moteur 0
     78      float voltage;
     79      GetCurrentSpeedAndVoltage(currents[i], speeds[i], voltage);
     80      battery->SetBatteryValue(voltage);
     81      last_voltage_time = GetTime();
     82    } else {
     83      GetCurrentAndSpeed(currents[i], speeds[i]);
    7084    }
    71 
    72     for(int i=0;i<nb_mot;i++) {
    73         i2cport->SetSlave(BASE_ADDRESS+i);
    74 
    75         if(i==0 && GetTime()>(last_voltage_time+5*(Time)1000000000)) {
    76             //toute les 5 secondes sur moteur 0
    77             float voltage;
    78             GetCurrentSpeedAndVoltage(currents[i],speeds[i],voltage);
    79             battery->SetBatteryValue(voltage);
    80             last_voltage_time=GetTime();
    81         } else {
    82             GetCurrentAndSpeed(currents[i],speeds[i]);
    83         }
     85  }
     86  // printf("%f %f %f %f\n",speeds[0],speeds[1],speeds[2],speeds[3]);
     87  /*
     88      if(GetTime()>(last_voltage_time+5*(Time)1000000000))//toute les 5 secondes
     89      {
     90          i2cport->SetSlave(BASE_ADDRESS);
     91          battery->SetBatteryValue(ReadVoltage());
     92          last_voltage_time=GetTime();
     93      }
     94  */
     95  i2cport->ReleaseMutex();
     96
     97  // on prend une fois pour toute le mutex et on fait des accès directs
     98  cvmatrix *output = self->output;
     99  output->GetMutex();
     100  for (int i = 0; i < nb_mot; i++)
     101    output->SetValueNoMutex(i, 0, speeds[i]);
     102  for (int i = 0; i < nb_mot; i++)
     103    output->SetValueNoMutex(i, 1, currents[i]);
     104  output->ReleaseMutex();
     105}
     106
     107// I2cPort mutex must be taken before calling this function
     108void BlCtrlV2_impl::WriteValue(uint16_t value) {
     109  uint8_t tx[2];
     110  ssize_t written;
     111
     112  tx[0] = (uint8_t)(value >> 3); // msb
     113  tx[1] = (value & 0x07);
     114  ; //+16+8; //pour recuperer la vitesse en premier
     115  written = i2cport->Write(tx, 2);
     116  if (written < 0) {
     117    self->Err("rt_dev_write error (%s)\n", strerror(-written));
     118  } else if (written != sizeof(tx)) {
     119    self->Err("rt_dev_write error %i/%i\n", written, sizeof(tx));
     120  }
     121}
     122
     123// I2cPort mutex must be taken before calling this function
     124void BlCtrlV2_impl::GetCurrentAndSpeed(float &current, float &speed) {
     125  ssize_t read;
     126  uint8_t value[4];
     127  read = i2cport->Read(value, sizeof(value));
     128
     129  if (read < 0) {
     130    self->Err("rt_dev_read error (%s)\n", strerror(-read));
     131    speed = -1;
     132    current = -1;
     133  } else if (read != sizeof(value)) {
     134    self->Err("rt_dev_read error %i/%i\n", read, sizeof(value));
     135    speed = -1;
     136    current = -1;
     137  } else {
     138    current = value[0] / 10.;
     139    speed = value[3] * 780. / poles->Value();
     140  }
     141}
     142
     143// I2cPort mutex must be taken before calling this function
     144void BlCtrlV2_impl::GetCurrentSpeedAndVoltage(float &current, float &speed,
     145                                              float &voltage) {
     146  ssize_t read;
     147  uint8_t value[6];
     148  read = i2cport->Read(value, sizeof(value));
     149
     150  if (read < 0) {
     151    self->Err("rt_dev_read error (%s)\n", strerror(-read));
     152    speed = -1;
     153    voltage = -1;
     154    current = -1;
     155  } else if (read != sizeof(value)) {
     156    self->Err("rt_dev_read error %i/%i\n", read, sizeof(value));
     157    speed = -1;
     158    voltage = -1;
     159    current = -1;
     160  } else {
     161    current = value[0] / 10.;
     162    voltage = value[5] / 10.;
     163    speed = value[3] * 780. / poles->Value();
     164  }
     165}
     166
     167void BlCtrlV2_impl::DetectMotors(void) {
     168  int nb = 0;
     169  ssize_t read, nb_write;
     170  uint8_t value[3];
     171  uint8_t tx[2];
     172
     173  i2cport->GetMutex();
     174
     175  for (int i = 0; i < MAX_MOTORS; i++) {
     176    // printf("test %i\n",i);
     177    i2cport->SetSlave(BASE_ADDRESS + i);
     178    tx[0] = 0;
     179    tx[1] = 16 + 8; // 16+8 pour recuperer la vitesse
     180
     181    nb_write = i2cport->Write(tx, 2);
     182
     183    if (nb_write != sizeof(tx)) {
     184      continue;
    84185    }
    85 //printf("%f %f %f %f\n",speeds[0],speeds[1],speeds[2],speeds[3]);
    86 /*
    87     if(GetTime()>(last_voltage_time+5*(Time)1000000000))//toute les 5 secondes
    88     {
    89         i2cport->SetSlave(BASE_ADDRESS);
    90         battery->SetBatteryValue(ReadVoltage());
    91         last_voltage_time=GetTime();
     186    nb++;
     187  }
     188
     189  for (int i = 0; i < MAX_MOTORS; i++) {
     190    i2cport->SetSlave(BASE_ADDRESS + i);
     191    read = i2cport->Read(value, 3);
     192
     193    if (read != sizeof(value)) {
     194      continue;
    92195    }
    93 */
    94     i2cport->ReleaseMutex();
    95 
    96     //on prend une fois pour toute le mutex et on fait des accès directs
    97     cvmatrix* output=self->output;
    98     output->GetMutex();
    99     for(int i=0;i<nb_mot;i++) output->SetValueNoMutex(i,0,speeds[i]);
    100     for(int i=0;i<nb_mot;i++) output->SetValueNoMutex(i,1,currents[i]);
    101     output->ReleaseMutex();
    102 }
    103 
    104 //I2cPort mutex must be taken before calling this function
    105 void BlCtrlV2_impl::WriteValue(uint16_t value) {
    106     uint8_t tx[2];
    107     ssize_t written;
    108 
    109     tx[0]=(uint8_t)(value>>3);//msb
    110     tx[1]=(value&0x07);;//+16+8; //pour recuperer la vitesse en premier
    111     written =i2cport->Write(tx, 2);
    112     if(written<0) {
    113         self->Err("rt_dev_write error (%s)\n",strerror(-written));
    114     } else if (written != sizeof(tx)) {
    115         self->Err("rt_dev_write error %i/%i\n",written,sizeof(tx));
    116     }
    117 }
    118 
    119 //I2cPort mutex must be taken before calling this function
    120 void BlCtrlV2_impl::GetCurrentAndSpeed(float &current,float &speed) {
    121     ssize_t read;
    122     uint8_t value[4];
    123     read = i2cport->Read(value, sizeof(value));
    124 
    125     if(read<0) {
    126         self->Err("rt_dev_read error (%s)\n",strerror(-read));
    127         speed=-1;
    128         current=-1;
    129     } else if (read != sizeof(value)) {
    130         self->Err("rt_dev_read error %i/%i\n",read,sizeof(value));
    131         speed=-1;
    132         current=-1;
    133     } else {
    134         current=value[0]/10.;
    135         speed= value[3]*780./poles->Value();
    136     }
    137 }
    138 
    139 //I2cPort mutex must be taken before calling this function
    140 void BlCtrlV2_impl::GetCurrentSpeedAndVoltage(float &current,float &speed,float &voltage) {
    141     ssize_t read;
    142     uint8_t value[6];
    143     read = i2cport->Read(value, sizeof(value));
    144 
    145     if(read<0) {
    146         self->Err("rt_dev_read error (%s)\n",strerror(-read));
    147         speed=-1;
    148         voltage=-1;
    149         current=-1;
    150     } else if (read != sizeof(value)) {
    151         self->Err("rt_dev_read error %i/%i\n",read,sizeof(value));
    152         speed=-1;
    153         voltage=-1;
    154         current=-1;
    155     } else {
    156         current=value[0]/10.;
    157         voltage=value[5]/10.;
    158         speed= value[3]*780./poles->Value();
    159     }
    160 }
    161 
    162 void BlCtrlV2_impl::DetectMotors(void) {
    163     int nb=0;
    164     ssize_t read,nb_write;
    165     uint8_t value[3];
    166     uint8_t tx[2];
    167 
    168     i2cport->GetMutex();
    169 
    170     for(int i=0;i<MAX_MOTORS;i++) {
    171         //printf("test %i\n",i);
    172         i2cport->SetSlave(BASE_ADDRESS+i);
    173         tx[0]=0;
    174         tx[1]=16+8;//16+8 pour recuperer la vitesse
    175 
    176         nb_write = i2cport->Write(tx, 2);
    177 
    178         if (nb_write != sizeof(tx)) {
    179             continue;
    180         }
    181         nb++;
    182     }
    183 
    184     for(int i=0;i<MAX_MOTORS;i++) {
    185         i2cport->SetSlave(BASE_ADDRESS+i);
    186         read = i2cport->Read(value, 3);
    187 
    188         if (read != sizeof(value)) {
    189             continue;
    190         }
    191     }
    192 
    193     i2cport->ReleaseMutex();
    194 
    195     Printf("BlCtrlV2: Dectected motors: %i\n",nb);
    196     if(nb==4) {
    197         Printf("BlCtrlV2: Configuration X4\n");
    198     } else if(nb==8) {
    199         Printf("BlCtrlV2: Configuration X8\n");
    200     } else {
    201         //self->Err("Error, configuration not supported (%i/%i)\n",nb,MAX_MOTORS);
    202     }
    203 
    204     nb_mot=nb;
    205 }
     196  }
     197
     198  i2cport->ReleaseMutex();
     199
     200  Printf("BlCtrlV2: Dectected motors: %i\n", nb);
     201  if (nb == 4) {
     202    Printf("BlCtrlV2: Configuration X4\n");
     203  } else if (nb == 8) {
     204    Printf("BlCtrlV2: Configuration X8\n");
     205  } else {
     206    // self->Err("Error, configuration not supported (%i/%i)\n",nb,MAX_MOTORS);
     207  }
     208
     209  nb_mot = nb;
     210}
Note: See TracChangeset for help on using the changeset viewer.