source: flair-src/trunk/lib/FlairSensorActuator/src/AfroBldc_impl.cpp @ 4

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

sensoractuator

File size: 1.8 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:    2015/06/15
6//  filename:   AfroBldc_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 "AfroBldc_impl.h"
18#include "AfroBldc.h"
19#include "Bldc_impl.h"
20#include "I2cPort.h"
21#include <Layout.h>
22#include <SpinBox.h>
23#include <Label.h>
24#include <cvmatrix.h>
25#include <string.h>
26
27#define BASE_ADDRESS 0x29
28#define MAX_VALUE 2047
29
30using std::string;
31using namespace flair::core;
32using namespace flair::gui;
33using namespace flair::sensor;
34using namespace flair::actuator;
35
36AfroBldc_impl::AfroBldc_impl(AfroBldc* self,Layout *layout,I2cPort* i2cport) {
37    this->self=self;
38    this->i2cport=i2cport;
39    nb_mot=self->MotorsCount();
40}
41
42AfroBldc_impl::~AfroBldc_impl() {
43
44}
45
46void AfroBldc_impl::SetMotors(float* value) {
47    uint16_t tosend_value[nb_mot];
48
49    for(int i=0; i<nb_mot; i++) tosend_value[i]=(uint16_t)(MAX_VALUE*value[i]);
50
51    i2cport->GetMutex();
52
53    for(int i=0; i<nb_mot; i++) {
54        i2cport->SetSlave(BASE_ADDRESS+i);
55        WriteValue(tosend_value[i]);
56    }
57    i2cport->ReleaseMutex();
58}
59
60//I2cPort mutex must be taken before calling this function
61void AfroBldc_impl::WriteValue(uint16_t value) {
62    uint8_t tx[2];
63    ssize_t written;
64
65    tx[0]=(uint8_t)(value>>3);//msb
66    tx[1]=(value&0x07);//lsb
67    written =i2cport->Write(tx, 2);
68    if(written<0) {
69        self->Err("rt_dev_write error (%s)\n",strerror(-written));
70    } else if (written != sizeof(tx)) {
71        self->Err("rt_dev_write error %i/%i\n",written,sizeof(tx));
72    }
73}
Note: See TracBrowser for help on using the repository browser.