Hello mates,
I'm doing a project with arduino uno and what I want to do is control my omni directional robot with a bluetooth application, in the past I already managed to do this but something happened with my code that now whenever "1" sending a command he receives as "49" table ascii.
Someone can tell me why? or how to fix this?
Here is the code:
#include <Wire.h> // required by Omni3MD.cpp
#include <Omni3MD.h>
//constants definitions
#define OMNI3MD_ADDRESS 0x30 // default factory address
Omni3MD omni; //declaration of object variable to control the Omni3MD
#define M1 1 //Motor1
#define M2 2 //Motor2
#define M3 3 //Motor3
// Void Setup (definicoes dos pinos)
int enc1=0; // encoder1 reading, this is the encoder incremental count for the defined prescaler (positional control)
int enc2=0; // encoder2 reading, this is the encoder incremental count for the defined prescaler (positional control)
int enc3=0; // encoder3 reading, this is the encoder incremental count for the defined prescaler (positional control)
float battery=0; // battery reading
float temperature=0; // temperature reading
byte firm_int=0; // the firmware version of your Omni3MD board (visit Omni-3MD to check if your version is updated)
byte firm_dec=0; // the firmware version of your Omni3MD board (visit Omni-3MD to check if your version is updated)
byte firm_dev=0; // the firmware version of your Omni3MD board (visit Omni-3MD to check if your version is updated)
byte ctrl_rate; // the control rate for your motors defined at calibration (in times per second)
int enc1_max; // maximum count for encoder 1 at calibration, for the defined control rate
int enc2_max; // maximum count for encoder 2 at calibration, for the defined control rate
int enc3_max; // maximum count for encoder 3 at calibration, for the defined control rate
void setup ()
{
//setup routines
Serial.begin(9600); // set baud rate to 115200bps for printing values in serial monitor. Press (ctrl+shift+m) after uploading
omni.i2c_connect(OMNI3MD_ADDRESS); // set i2c connection
delay(10); // pause 10 milliseconds
omni.stop_motors(); // stops all motors
delay(10);
omni.set_i2c_timeout(0); // safety parameter -> I2C communication must occur every [byte timeout] x 100 miliseconds for motor movement
delay(5); // 5ms pause required for Omni3MD eeprom writing
omni.set_PID(250,400,125); // Adjust paramenters for PID control [word Kp, word Ki, word Kd]
delay(15); // 15ms pause required for Omni3MD eeprom writing
omni.set_ramp(500,1500,1000); // set acceleration ramp and limiar take off parameter gain[word ramp_time, word slope, word Kl]
delay(15); // 10ms pause required for Omni3MD eeprom writing
omni.read_firmware(&firm_int,&firm_dec,&firm_dev); // read firmware version value
Serial.print("Firmware:");
Serial.print(firm_int); // prints firmware value
Serial.print(".");
Serial.print(firm_dec); // prints firmware value
Serial.print(".");
Serial.println(firm_dev); // prints firmware value
ctrl_rate=omni.read_control_rate(); // read the control rate value
Serial.print("Control_Rate:");
Serial.println(ctrl_rate); // prints control rate value
enc1_max=omni.read_enc1_max(); // read encoder1 maximum value at calibration (usefull for detecting a faulty encoder)
Serial.print("Encoder1_max:");
Serial.println(enc1_max); // prints encoder1 maximum calibration value
enc2_max=omni.read_enc2_max(); // read encoder1 maximum value at calibration (usefull for detecting a faulty encoder)
Serial.print("Encoder2_max:");
Serial.println(enc2_max); // prints encoder2 maximum calibration value
enc3_max=omni.read_enc3_max(); // read encoder1 maximum value at calibration (usefull for detecting a faulty encoder)
Serial.print("Encoder3_max:");
Serial.println(enc3_max); // prints encoder3 maximum calibration value
battery=omni.read_battery(); // read battery value
Serial.print("Battery:");
Serial.println(battery); // prints battery value
temperature=omni.read_temperature(); // read temperature value
Serial.print("Temperature:");
Serial.println(temperature); // prints temperature value
delay(2000);
omni.stop_motors();
}
//Print values in serial monitor
void print_enc_values()
{
Serial.print("enc1:");
Serial.print(enc1); // prints encoder 1 positional value
Serial.print(" enc2:");
Serial.print(enc2); // prints encoder 2 positional value
Serial.print(" enc3:");
Serial.println(enc3); // prints encoder 3 positional value
}
void wait(int time)
{
Serial.println();
for(int i=0;i<time;i++) //10->1s
{
omni.read_encoders(&enc1,&enc2,&enc3);
print_enc_values(); //Print values in serial monitor
delay(200);
}
}
void loop ()
{//inicio do void loop
if (Serial.available())
{
int md = Serial.read();
if(md==1)
{
omni.mov_lin3m_pid(50,-50,0);
}
else if(md==2)
{
omni.mov_lin3m_pid(-50,50,0);
}
else if(md==3)
{
omni.mov_lin3m_pid(30,30,30);
}
else if(md==4)
{
omni.mov_lin3m_pid(-30,-30,-30);
}
else omni.mov_lin3m_pid(0,0,0);
Serial.println(md);
}