00001
00002
00003
00004
00005
00006
00007
00008 #include "Wire.h"
00009 #include "Omni3MD.h"
00010
00011
00012
00013
00014
00015
00017
00019 void Omni3MD::i2c_connect(byte omniAddress)
00020 {
00021 _omniAddress = omniAddress>>1;
00022 Wire.begin(_omniAddress);
00023 }
00024
00025 byte Omni3MD::i2cRequestData(byte addressValue, byte command)
00026 {
00027 byte value=(byte)0xFF;
00028 Wire.beginTransmission(addressValue);
00029 Wire.send(command);
00030 Wire.endTransmission();
00031
00032 Wire.requestFrom((int)addressValue,(int)1);
00033 if (Wire.available())
00034 {
00035 value=Wire.receive();
00036 }
00037 return value;
00038 }
00039
00040 void Omni3MD::i2cSendData(byte addressValue, byte command, byte buffer[], byte numBytes)
00041 {
00042 byte value=0;
00043 Wire.beginTransmission(addressValue);
00044 Wire.send(command);
00045 for (int k =0; k< numBytes;k++)
00046 Wire.send(buffer[k]);
00047 Wire.endTransmission();
00048 }
00049
00051
00053
00054 void Omni3MD::calibrate_omni()
00055 {
00056 byte buffer[]={KEY1,KEY2};
00057 i2cSendData(_omniAddress, COMMAND_CALIBRATE,buffer,sizeof(buffer));
00058 }
00059 void Omni3MD::set_i2c_timeout (byte timeout)
00060 {
00061 byte buffer[]={timeout, KEY1, timeout, KEY2};
00062 i2cSendData(_omniAddress, COMMAND_TIMEOUT_I2C,buffer,sizeof(buffer));
00063 }
00064 void Omni3MD::set_i2c_address (byte newAddress)
00065 {
00066 byte buffer[]={newAddress, KEY1, newAddress, KEY2};
00067 i2cSendData(_omniAddress, COMMAND_I2C_ADD,buffer,sizeof(buffer));
00068 _omniAddress=newAddress;
00069 }
00070
00071
00072
00073
00074
00075 void Omni3MD::set_PID(word Kp, word Ki, word Kd)
00076 {
00077 byte Kp_H=highByte(Kp);
00078 byte Kp_L=lowByte(Kp);
00079 byte Ki_H=highByte(Ki);
00080 byte Ki_L=lowByte(Ki);
00081 byte Kd_H=highByte(Kd);
00082 byte Kd_L=lowByte(Kd);
00083 byte buffer[]={Kp_H,Kp_L,Ki_H,Ki_L,Kd_H,Kd_L};
00084 i2cSendData(_omniAddress, COMMAND_GAIN_UPDATE, buffer, sizeof(buffer));
00085 }
00086 void Omni3MD::set_prescaler(byte encoder, byte value)
00087 {
00088 byte buffer[]={encoder,value,KEY1,KEY2};
00089 i2cSendData(_omniAddress, COMMAND_PRESCALER_CFG,buffer,sizeof(buffer));
00090 }
00091 void Omni3MD::set_enc_value(byte encoder, word encValue)
00092 {
00093 byte encValueHIGH=highByte(encValue);
00094 byte encValueLOW=lowByte(encValue);
00095 byte buffer[]={encoder,encValueHIGH,encValueLOW,KEY1,KEY2};
00096 i2cSendData(_omniAddress, COMMAND_INC_ENC_PRESET,buffer,sizeof(buffer));
00097 }
00098
00100
00102
00103
00104
00105
00106
00107
00108
00109
00110
00111
00112
00113
00114
00115
00116
00117 void Omni3MD::mov_lin3M_pid(byte way1,byte speed1,byte way2,byte speed2,byte way3,byte speed3)
00118 {
00119 byte buffer[]={way1,speed1,way2,speed2,way3,speed3};
00120 i2cSendData(_omniAddress, COMMAND_MOV_LIN3M_PID,buffer,sizeof(buffer));
00121 }
00122
00123
00124
00125
00126
00127
00128
00129
00130
00131
00132
00133
00134
00135
00136
00137
00138
00139
00140
00141
00142
00143
00144 void Omni3MD::stop_motors()
00145 {
00146 byte buffer[]={KEY1,KEY2};
00147 i2cSendData(_omniAddress, COMMAND_STOP,buffer,sizeof(buffer));
00148 }
00149
00151
00153
00154 float Omni3MD::read_temperature()
00155 {
00156 byte temperHigh=i2cRequestData(_omniAddress, COMMAND_TEMPER1_HI);
00157 byte temperLow=i2cRequestData(_omniAddress, COMMAND_TEMPER1_LOW);
00158 float temperatureValue=((int)temperHigh*255)+(int)temperLow;
00159 return (temperatureValue/10);
00160 }
00161 float Omni3MD::read_battery()
00162 {
00163 byte batteryValueLow=i2cRequestData(_omniAddress, COMMAND_BAT_LOW);
00164 byte batteryValueHigh=i2cRequestData(_omniAddress, COMMAND_BAT_HI);
00165
00166 return(((batteryValueHigh<<8)+batteryValueLow)/10.0);
00167 }
00168 float Omni3MD::read_firmware()
00169 {
00170 byte firmI=i2cRequestData(_omniAddress, COMMAND_FIRMWARE_INT);
00171 byte firmD=i2cRequestData(_omniAddress, COMMAND_FIRMWARE_DEC);
00172 float firmwareValue=(firmI<<8)+firmD;
00173 return (firmwareValue/100.0);
00174 }
00175
00176
00177
00178
00179
00180
00181
00182
00183
00184
00185
00186
00187
00188
00189
00190
00191
00192
00193
00194
00195
00196
00197
00198
00199
00200
00201
00202
00203
00204
00205
00206
00207
00208
00209
00210
00211
00212
00213
00214
00215 int Omni3MD::read_enc1()
00216 {
00217 byte enc1_inc_hi=i2cRequestData(_omniAddress, COMMAND_ENC1_INC_HI);
00218 byte enc1_inc_low=i2cRequestData(_omniAddress, COMMAND_ENC1_INC_LOW);
00219 return ((enc1_inc_hi<<8)+enc1_inc_low);
00220 }
00221 int Omni3MD::read_enc2()
00222 {
00223 byte enc2_inc_hi=i2cRequestData(_omniAddress, COMMAND_ENC2_INC_HI);
00224 byte enc2_inc_low=i2cRequestData(_omniAddress, COMMAND_ENC2_INC_LOW);
00225 return ((enc2_inc_hi<<8)+enc2_inc_low);
00226 }
00227
00228
00229
00230
00231
00232