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

Last change on this file since 330 was 268, checked in by Sanahuja Guillaume, 3 years ago

add defines for architectures to speed up compile time

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