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

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

sensoractuator

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#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 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
53BlCtrlV2_impl::~BlCtrlV2_impl() {
54
55}
56
57void 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]);
70    }
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        }
84    }
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();
92    }
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
105void 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
120void 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
140void 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
162void 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}
Note: See TracBrowser for help on using the repository browser.