[3] | 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: 2011/09/13
|
---|
| 6 | // filename: BlCtrlV2_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 "BlCtrlV2_impl.h"
|
---|
| 18 | #include "BlCtrlV2.h"
|
---|
| 19 | #include "Bldc_impl.h"
|
---|
| 20 | #include "BatteryMonitor.h"
|
---|
| 21 | #include "I2cPort.h"
|
---|
| 22 | #include <Layout.h>
|
---|
| 23 | #include <SpinBox.h>
|
---|
| 24 | #include <Label.h>
|
---|
| 25 | #include <cvmatrix.h>
|
---|
| 26 | #include <string.h>
|
---|
| 27 |
|
---|
| 28 | #define BASE_ADDRESS 0x29
|
---|
| 29 | #define MAX_VALUE 2047
|
---|
| 30 |
|
---|
| 31 | using std::string;
|
---|
| 32 | using namespace flair::core;
|
---|
| 33 | using namespace flair::gui;
|
---|
| 34 | using namespace flair::sensor;
|
---|
| 35 | using namespace flair::actuator;
|
---|
| 36 |
|
---|
| 37 | BlCtrlV2_impl::BlCtrlV2_impl(BlCtrlV2* self,Layout *layout,I2cPort* i2cport) {
|
---|
| 38 | this->self=self;
|
---|
| 39 | this->i2cport=i2cport;
|
---|
| 40 | last_voltage_time=0;
|
---|
| 41 |
|
---|
| 42 | DetectMotors();
|
---|
| 43 | //if(nb_mot!=self->MotorsCount()) self->Err("motors count different from multiplex count\n");
|
---|
| 44 |
|
---|
| 45 | //BatteryMonitor
|
---|
| 46 | battery=new BatteryMonitor(layout->NewRow(),"battery");
|
---|
| 47 |
|
---|
| 48 | //user interface
|
---|
| 49 | GroupBox *setupgroupbox=new GroupBox(layout->NewRow(),"Motors");
|
---|
| 50 | poles=new SpinBox(setupgroupbox->NewRow(),"nb poles:",0,255,1,14);
|
---|
| 51 | }
|
---|
| 52 |
|
---|
| 53 | BlCtrlV2_impl::~BlCtrlV2_impl() {
|
---|
| 54 |
|
---|
| 55 | }
|
---|
| 56 |
|
---|
| 57 | void BlCtrlV2_impl::SetMotors(float* value) {
|
---|
| 58 | uint16_t tosend_value[nb_mot];
|
---|
| 59 | //stocke dans une variable pour garder le moins longtemps l'i2c (pour ne pas bloquer sur le mutex de l'output)
|
---|
| 60 | float speeds[nb_mot];
|
---|
| 61 | float currents[nb_mot];
|
---|
| 62 |
|
---|
| 63 | for(int i=0;i<nb_mot;i++) tosend_value[i]=(uint16_t)(MAX_VALUE*value[i]);
|
---|
| 64 |
|
---|
| 65 | i2cport->GetMutex();
|
---|
| 66 |
|
---|
| 67 | for(int i=0;i<nb_mot;i++) {
|
---|
| 68 | i2cport->SetSlave(BASE_ADDRESS+i);
|
---|
| 69 | WriteValue(tosend_value[i]);
|
---|
| 70 | }
|
---|
| 71 |
|
---|
| 72 | for(int i=0;i<nb_mot;i++) {
|
---|
| 73 | i2cport->SetSlave(BASE_ADDRESS+i);
|
---|
| 74 |
|
---|
| 75 | if(i==0 && GetTime()>(last_voltage_time+5*(Time)1000000000)) {
|
---|
| 76 | //toute les 5 secondes sur moteur 0
|
---|
| 77 | float voltage;
|
---|
| 78 | GetCurrentSpeedAndVoltage(currents[i],speeds[i],voltage);
|
---|
| 79 | battery->SetBatteryValue(voltage);
|
---|
| 80 | last_voltage_time=GetTime();
|
---|
| 81 | } else {
|
---|
| 82 | GetCurrentAndSpeed(currents[i],speeds[i]);
|
---|
| 83 | }
|
---|
| 84 | }
|
---|
| 85 | //printf("%f %f %f %f\n",speeds[0],speeds[1],speeds[2],speeds[3]);
|
---|
| 86 | /*
|
---|
| 87 | if(GetTime()>(last_voltage_time+5*(Time)1000000000))//toute les 5 secondes
|
---|
| 88 | {
|
---|
| 89 | i2cport->SetSlave(BASE_ADDRESS);
|
---|
| 90 | battery->SetBatteryValue(ReadVoltage());
|
---|
| 91 | last_voltage_time=GetTime();
|
---|
| 92 | }
|
---|
| 93 | */
|
---|
| 94 | i2cport->ReleaseMutex();
|
---|
| 95 |
|
---|
| 96 | //on prend une fois pour toute le mutex et on fait des accès directs
|
---|
| 97 | cvmatrix* output=self->output;
|
---|
| 98 | output->GetMutex();
|
---|
| 99 | for(int i=0;i<nb_mot;i++) output->SetValueNoMutex(i,0,speeds[i]);
|
---|
| 100 | for(int i=0;i<nb_mot;i++) output->SetValueNoMutex(i,1,currents[i]);
|
---|
| 101 | output->ReleaseMutex();
|
---|
| 102 | }
|
---|
| 103 |
|
---|
| 104 | //I2cPort mutex must be taken before calling this function
|
---|
| 105 | void BlCtrlV2_impl::WriteValue(uint16_t value) {
|
---|
| 106 | uint8_t tx[2];
|
---|
| 107 | ssize_t written;
|
---|
| 108 |
|
---|
| 109 | tx[0]=(uint8_t)(value>>3);//msb
|
---|
| 110 | tx[1]=(value&0x07);;//+16+8; //pour recuperer la vitesse en premier
|
---|
| 111 | written =i2cport->Write(tx, 2);
|
---|
| 112 | if(written<0) {
|
---|
| 113 | self->Err("rt_dev_write error (%s)\n",strerror(-written));
|
---|
| 114 | } else if (written != sizeof(tx)) {
|
---|
| 115 | self->Err("rt_dev_write error %i/%i\n",written,sizeof(tx));
|
---|
| 116 | }
|
---|
| 117 | }
|
---|
| 118 |
|
---|
| 119 | //I2cPort mutex must be taken before calling this function
|
---|
| 120 | void BlCtrlV2_impl::GetCurrentAndSpeed(float ¤t,float &speed) {
|
---|
| 121 | ssize_t read;
|
---|
| 122 | uint8_t value[4];
|
---|
| 123 | read = i2cport->Read(value, sizeof(value));
|
---|
| 124 |
|
---|
| 125 | if(read<0) {
|
---|
| 126 | self->Err("rt_dev_read error (%s)\n",strerror(-read));
|
---|
| 127 | speed=-1;
|
---|
| 128 | current=-1;
|
---|
| 129 | } else if (read != sizeof(value)) {
|
---|
| 130 | self->Err("rt_dev_read error %i/%i\n",read,sizeof(value));
|
---|
| 131 | speed=-1;
|
---|
| 132 | current=-1;
|
---|
| 133 | } else {
|
---|
| 134 | current=value[0]/10.;
|
---|
| 135 | speed= value[3]*780./poles->Value();
|
---|
| 136 | }
|
---|
| 137 | }
|
---|
| 138 |
|
---|
| 139 | //I2cPort mutex must be taken before calling this function
|
---|
| 140 | void BlCtrlV2_impl::GetCurrentSpeedAndVoltage(float ¤t,float &speed,float &voltage) {
|
---|
| 141 | ssize_t read;
|
---|
| 142 | uint8_t value[6];
|
---|
| 143 | read = i2cport->Read(value, sizeof(value));
|
---|
| 144 |
|
---|
| 145 | if(read<0) {
|
---|
| 146 | self->Err("rt_dev_read error (%s)\n",strerror(-read));
|
---|
| 147 | speed=-1;
|
---|
| 148 | voltage=-1;
|
---|
| 149 | current=-1;
|
---|
| 150 | } else if (read != sizeof(value)) {
|
---|
| 151 | self->Err("rt_dev_read error %i/%i\n",read,sizeof(value));
|
---|
| 152 | speed=-1;
|
---|
| 153 | voltage=-1;
|
---|
| 154 | current=-1;
|
---|
| 155 | } else {
|
---|
| 156 | current=value[0]/10.;
|
---|
| 157 | voltage=value[5]/10.;
|
---|
| 158 | speed= value[3]*780./poles->Value();
|
---|
| 159 | }
|
---|
| 160 | }
|
---|
| 161 |
|
---|
| 162 | void BlCtrlV2_impl::DetectMotors(void) {
|
---|
| 163 | int nb=0;
|
---|
| 164 | ssize_t read,nb_write;
|
---|
| 165 | uint8_t value[3];
|
---|
| 166 | uint8_t tx[2];
|
---|
| 167 |
|
---|
| 168 | i2cport->GetMutex();
|
---|
| 169 |
|
---|
| 170 | for(int i=0;i<MAX_MOTORS;i++) {
|
---|
| 171 | //printf("test %i\n",i);
|
---|
| 172 | i2cport->SetSlave(BASE_ADDRESS+i);
|
---|
| 173 | tx[0]=0;
|
---|
| 174 | tx[1]=16+8;//16+8 pour recuperer la vitesse
|
---|
| 175 |
|
---|
| 176 | nb_write = i2cport->Write(tx, 2);
|
---|
| 177 |
|
---|
| 178 | if (nb_write != sizeof(tx)) {
|
---|
| 179 | continue;
|
---|
| 180 | }
|
---|
| 181 | nb++;
|
---|
| 182 | }
|
---|
| 183 |
|
---|
| 184 | for(int i=0;i<MAX_MOTORS;i++) {
|
---|
| 185 | i2cport->SetSlave(BASE_ADDRESS+i);
|
---|
| 186 | read = i2cport->Read(value, 3);
|
---|
| 187 |
|
---|
| 188 | if (read != sizeof(value)) {
|
---|
| 189 | continue;
|
---|
| 190 | }
|
---|
| 191 | }
|
---|
| 192 |
|
---|
| 193 | i2cport->ReleaseMutex();
|
---|
| 194 |
|
---|
| 195 | Printf("BlCtrlV2: Dectected motors: %i\n",nb);
|
---|
| 196 | if(nb==4) {
|
---|
| 197 | Printf("BlCtrlV2: Configuration X4\n");
|
---|
| 198 | } else if(nb==8) {
|
---|
| 199 | Printf("BlCtrlV2: Configuration X8\n");
|
---|
| 200 | } else {
|
---|
| 201 | //self->Err("Error, configuration not supported (%i/%i)\n",nb,MAX_MOTORS);
|
---|
| 202 | }
|
---|
| 203 |
|
---|
| 204 | nb_mot=nb;
|
---|
| 205 | }
|
---|