source: flair-src/trunk/lib/FlairSensorActuator/src/XBldc_impl.cpp @ 157

Last change on this file since 157 was 15, checked in by Bayard Gildas, 6 years ago

sources reformatted with flair-format-dir script

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