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

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

sensoractuator

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}
40
41void XBldc_impl::SetMotors(float* value) {
42 uint8_t tosend_value[5];
43
44 for(int i=0;i<4;i++) 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++) tosend_value[4]+=tosend_value[i];
49 self->Err("please verify motors adresses\n");
50 //todo faire la procedure changement addresse par la station sol
51/*
52 i2cport->GetMutex();
53 i2cport->SetSlave(BASE_ADDRESS);
54
55 written =i2cport->Write(tosend_value, sizeof(tosend_value));
56 if(written<0)
57 {
58 self->Err("rt_dev_write error (%s)\n",strerror(-written));
59 }
60 else if (written != sizeof(tosend_value))
61 {
62 self->Err("rt_dev_write error %i/%i\n",written,sizeof(tosend_value));
63 }
64
65 i2cport->ReleaseMutex();*/
66
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)) sat_value++;
143
144 if(value<(float)min) sat_value=min;
145 if(value>(float)max) sat_value=max;
146
147 return sat_value;
148}
Note: See TracBrowser for help on using the repository browser.