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

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

add defines for architectures to speed up compile time

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