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

Last change on this file since 251 was 214, checked in by Sanahuja Guillaume, 6 years ago

matrix

File size: 5.5 KB
RevLine 
[3]1// %flair:license{
[15]2// This file is part of the Flair framework distributed under the
3// CECILL-C License, Version 1.0.
[3]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>
[214]25#include <Matrix.h>
[3]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
[15]37BlCtrlV2_impl::BlCtrlV2_impl(BlCtrlV2 *self, Layout *layout, I2cPort *i2cport) {
38 this->self = self;
39 this->i2cport = i2cport;
40 last_voltage_time = 0;
[3]41
[15]42 DetectMotors();
43 // if(nb_mot!=self->MotorsCount()) self->Err("motors count different from
44 // multiplex count\n");
[3]45
[15]46 // BatteryMonitor
47 battery = new BatteryMonitor(layout->NewRow(), "battery");
[3]48
[15]49 // user interface
50 GroupBox *setupgroupbox = new GroupBox(layout->NewRow(), "Motors");
51 poles = new SpinBox(setupgroupbox->NewRow(), "nb poles:", 0, 255, 1, 14);
[3]52}
53
[15]54BlCtrlV2_impl::~BlCtrlV2_impl() {}
[3]55
[15]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];
[3]62
[15]63 for (int i = 0; i < nb_mot; i++)
64 tosend_value[i] = (uint16_t)(MAX_VALUE * value[i]);
[3]65
[15]66 i2cport->GetMutex();
[3]67
[15]68 for (int i = 0; i < nb_mot; i++) {
69 i2cport->SetSlave(BASE_ADDRESS + i);
70 WriteValue(tosend_value[i]);
71 }
[3]72
[15]73 for (int i = 0; i < nb_mot; i++) {
74 i2cport->SetSlave(BASE_ADDRESS + i);
[3]75
[15]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]);
[3]84 }
[15]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();
[3]96
[15]97 // on prend une fois pour toute le mutex et on fait des accès directs
[214]98 Matrix *output = self->output;
[15]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();
[3]105}
106
[15]107// I2cPort mutex must be taken before calling this function
[3]108void BlCtrlV2_impl::WriteValue(uint16_t value) {
[15]109 uint8_t tx[2];
110 ssize_t written;
[3]111
[15]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 }
[3]121}
122
[15]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));
[3]128
[15]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 }
[3]141}
142
[15]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));
[3]149
[15]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 }
[3]165}
166
167void BlCtrlV2_impl::DetectMotors(void) {
[15]168 int nb = 0;
169 ssize_t read, nb_write;
170 uint8_t value[3];
171 uint8_t tx[2];
[3]172
[15]173 i2cport->GetMutex();
[3]174
[15]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
[3]180
[15]181 nb_write = i2cport->Write(tx, 2);
[3]182
[15]183 if (nb_write != sizeof(tx)) {
184 continue;
[3]185 }
[15]186 nb++;
187 }
[3]188
[15]189 for (int i = 0; i < MAX_MOTORS; i++) {
190 i2cport->SetSlave(BASE_ADDRESS + i);
191 read = i2cport->Read(value, 3);
[3]192
[15]193 if (read != sizeof(value)) {
194 continue;
[3]195 }
[15]196 }
[3]197
[15]198 i2cport->ReleaseMutex();
[3]199
[15]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 }
[3]208
[15]209 nb_mot = nb;
[3]210}
Note: See TracBrowser for help on using the repository browser.