source: flair-src/trunk/lib/FlairSensorActuator/src/BlCtrlV2_impl.cpp @ 157

Last change on this file since 157 was 15, checked in by Bayard Gildas, 5 years ago

sources reformatted with flair-format-dir script

File size: 5.5 KB
Line 
1// %flair:license{
2// This file is part of the Flair framework distributed under the
3// CECILL-C License, Version 1.0.
4// %flair:license}
5//  created:    2011/09/13
6//  filename:   BlCtrlV2_impl.cpp
7//
8//  author:     Guillaume Sanahuja
9//              Copyright Heudiasyc UMR UTC/CNRS 7253
10//
11//  version:    $Id: $
12//
13//  purpose:    objet integrant les moteurs i2c
14//
15//
16/*********************************************************************/
17#include "BlCtrlV2_impl.h"
18#include "BlCtrlV2.h"
19#include "Bldc_impl.h"
20#include "BatteryMonitor.h"
21#include "I2cPort.h"
22#include <Layout.h>
23#include <SpinBox.h>
24#include <Label.h>
25#include <cvmatrix.h>
26#include <string.h>
27
28#define BASE_ADDRESS 0x29
29#define MAX_VALUE 2047
30
31using std::string;
32using namespace flair::core;
33using namespace flair::gui;
34using namespace flair::sensor;
35using namespace flair::actuator;
36
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]);
84    }
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;
185    }
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;
195    }
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 TracBrowser for help on using the repository browser.