source: flair-src/trunk/lib/FlairSensorActuator/src/XBldc_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: 3.4 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:    2013/11/13
6//  filename:   XBldc_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 Xufo
14//
15//
16/*********************************************************************/
17#ifdef ARMV7A
18
19#include "XBldc_impl.h"
20#include "XBldc.h"
21#include <I2cPort.h>
22#include <Matrix.h>
23#include <string.h>
24
25#define BASE_ADDRESS 0x01
26#define MAX_VALUE 200
27
28using namespace flair::core;
29using namespace flair::gui;
30using namespace flair::actuator;
31
32XBldc_impl::XBldc_impl(XBldc *self, I2cPort *i2cport) {
33  this->self = self;
34  this->i2cport = i2cport;
35
36  i2cport->SetTxTimeout(5000000);
37}
38
39XBldc_impl::~XBldc_impl() {}
40
41void XBldc_impl::SetMotors(float *value) {
42  uint8_t tosend_value[5];
43
44  for (int i = 0; i < 4; i++)
45    tosend_value[i] = (uint8_t)(MAX_VALUE * value[i]);
46
47  ssize_t written;
48  tosend_value[4] = 0xaa;
49  for (int i = 0; i < 4; i++)
50    tosend_value[4] += tosend_value[i];
51  self->Err("please verify motors adresses\n");
52  // todo faire la procedure changement addresse par la station sol
53  /*
54      i2cport->GetMutex();
55      i2cport->SetSlave(BASE_ADDRESS);
56
57      written =i2cport->Write(tosend_value, sizeof(tosend_value));
58      if(written<0)
59      {
60          self->Err("rt_dev_write error (%s)\n",strerror(-written));
61      }
62      else if (written != sizeof(tosend_value))
63      {
64          self->Err("rt_dev_write error %i/%i\n",written,sizeof(tosend_value));
65      }
66
67      i2cport->ReleaseMutex();*/
68}
69/*
70void XBldc_impl::StartTest(uint8_t moteur)
71{
72    unsigned char tx[4];
73    ssize_t written;
74
75    tx[0]=251;
76    tx[1]=moteur;
77    tx[2]=0;
78    tx[3]=231+moteur;
79
80    i2cport->GetMutex();
81    i2cport->SetSlave(BASE_ADDRESS);
82
83    written =i2cport->Write(tx, sizeof(tx));
84    if(written<0)
85    {
86        self->Err("rt_dev_write error (%s)\n",strerror(-written));
87    }
88    else if (written != sizeof(tx))
89    {
90        self->Err("rt_dev_write %i/%i error\n",written,sizeof(tx));
91    }
92
93    i2cport->ReleaseMutex();
94}
95*/
96void XBldc_impl::ChangeDirection(uint8_t moteur) {
97  unsigned char tx[4];
98  ssize_t written;
99
100  tx[0] = 254;
101  tx[1] = moteur;
102  tx[2] = 0;
103  tx[3] = 234 + moteur;
104
105  i2cport->GetMutex();
106  i2cport->SetSlave(BASE_ADDRESS);
107
108  written = i2cport->Write(tx, sizeof(tx));
109  if (written < 0) {
110    self->Err("rt_dev_write (%s) error\n", strerror(-written));
111  } else if (written != sizeof(tx)) {
112    self->Err("rt_dev_write %i/%i error\n", written, sizeof(tx));
113  }
114
115  i2cport->ReleaseMutex();
116}
117
118void XBldc_impl::ChangeAdress(uint8_t old_add, uint8_t new_add) {
119  unsigned char tx[4];
120  ssize_t written;
121
122  tx[0] = 250;
123  tx[1] = old_add;
124  tx[2] = new_add;
125  tx[3] = 230 + old_add + new_add;
126
127  i2cport->GetMutex();
128  i2cport->SetSlave(BASE_ADDRESS);
129
130  written = i2cport->Write(tx, sizeof(tx));
131  if (written < 0) {
132    self->Err("rt_dev_write error (%s)\n", strerror(-written));
133  } else if (written != sizeof(tx)) {
134    self->Err("rt_dev_write error %i/%i\n", written, sizeof(tx));
135  }
136
137  i2cport->ReleaseMutex();
138}
139
140uint8_t XBldc_impl::Sat(float value, uint8_t min, uint8_t max) {
141  uint8_t sat_value = (uint8_t)value;
142
143  if (value > ((float)sat_value + 0.5))
144    sat_value++;
145
146  if (value < (float)min)
147    sat_value = min;
148  if (value > (float)max)
149    sat_value = max;
150
151  return sat_value;
152}
153
154#endif
Note: See TracBrowser for help on using the repository browser.