Self balancing bluetooth

Salve, sto costruendo un robot autobilanciante e mi trovo in difficoltà nell’aggiungere il controllo bluetooth tramite android e un app con un joystik.

La grandezza da controllare sarebbe direttamente la velocità dei motori?
Inoltre, come invio i dati del joystik (X e Y) ad Arduino? So che non si possono inviare numeri di più cifre semplicemente.

Grazie

Cosmo99:
Inoltre, come invio i dati del joystik (X e Y) ad Arduino? So che non si possono inviare numeri di più cifre semplicemente.

?
Cosa intendi? In Android hai l’OutputStream dell’oggetto Socket con cui fai una Write di String.getBytes()… decidi tu un protocollo, ad es “<XCOORD|YCOORD>”

Io vorrei usare un’app android che invia con il bluetooth i dati del joystick. Come faccio ad usare queste coordinate per far muovere il robot, mentre si bilancia al tempo stesso? Ho provato in vari modi ma non ho ben capito come fare…

Questo è il codice, preso da un altro progetto e modificato per funzionare con la Shield e i motori pololu.

#include <Wire.h>

#include "DualVNH5019MotorShield.h"

DualVNH5019MotorShield md;

int state;
int vel;

/*MPU-6050 gives you 16 bits data so you have to create some 16int constants
   to store the data for accelerations and gyro*/

int16_t Acc_rawX, Acc_rawY, Acc_rawZ, Gyr_rawX, Gyr_rawY, Gyr_rawZ;


float Acceleration_angle[2];
float Gyro_angle[2];
float Total_angle[2];

float elapsedTime, time, timePrev;
int i;
float rad_to_deg = 180 / 3.141592654;

float PID, pwmLeft, pwmRight, error, previous_error;

float pid_p = 0;
float pid_i = 0;
float pid_d = 0;
/////////////////PID CONSTANTS/////////////////
double kp = 1.7525; //3.55
double ki = 5; //0.003
double kd = 0.389; //2.05
///////////////////////////////////////////////

double throttle = 0; //initial value of throttle to the motors  
float desired_angle = 0; //This is the angle in which we whant the
//balance to stay steady

void setup() {
  Wire.begin(); //begin the wire comunication
  Wire.beginTransmission(0x68);
  Wire.write(0x6B);
  Wire.write(0);
  Wire.endTransmission(true);
  Serial.begin(250000);
  md.init();
  Serial.begin(9600);

  time = millis(); //Start counting time in milliseconds
  /*In order to start up the ESCs we have to send a min value
     of PWM to them before connecting the battery. Otherwise
     the ESCs won't start up or enter in the configure mode.
     The min value is 1000us and max is 2000us, REMEMBER!*/
  //left_prop.writeMicroseconds(1000);
  // right_prop.writeMicroseconds(1000);
  // delay(7000); /*Give some delay, 7s, to have time to connect
  //    *the propellers and let everything start up*/
}//end of setup void


void loop() {


  // Serial.println('throttle');
  /////////////////////////////I M U/////////////////////////////////////
  timePrev = time;  // the previous time is stored before the actual time read
  time = millis();  // actual time read
  elapsedTime = (time - timePrev) / 1000;

  /*The tiemStep is the time that elapsed since the previous loop.
     This is the value that we will use in the formulas as "elapsedTime"
     in seconds. We work in ms so we haveto divide the value by 1000
    to obtain seconds*/

  /*Reed the values that the accelerometre gives.
     We know that the slave adress for this IMU is 0x68 in
     hexadecimal. For that in the RequestFrom and the
     begin functions we have to put this value.*/

  Wire.beginTransmission(0x68);
  Wire.write(0x3B); //Ask for the 0x3B register- correspond to AcX
  Wire.endTransmission(false);
  Wire.requestFrom(0x68, 6, true);

  /*We have asked for the 0x3B register. The IMU will send a brust of register.
     The amount of register to read is specify in the requestFrom function.
     In this case we request 6 registers. Each value of acceleration is made out of
     two 8bits registers, low values and high values. For that we request the 6 of them
     and just make then sum of each pair. For that we shift to the left the high values
     register (<<) and make an or (|) operation to add the low values.*/

  Acc_rawX = Wire.read() << 8 | Wire.read(); //each value needs two registres
  Acc_rawY = Wire.read() << 8 | Wire.read();
  Acc_rawZ = Wire.read() << 8 | Wire.read();



  /*---X---*/
  Acceleration_angle[0] = atan((Acc_rawY / 16384.0) / sqrt(pow((Acc_rawX / 16384.0), 2) + pow((Acc_rawZ / 16384.0), 2))) * rad_to_deg;
  /*---Y---*/
  Acceleration_angle[1] = atan(-1 * (Acc_rawX / 16384.0) / sqrt(pow((Acc_rawY / 16384.0), 2) + pow((Acc_rawZ / 16384.0), 2))) * rad_to_deg;


  Wire.beginTransmission(0x68);
  Wire.write(0x43); //Gyro data first adress
  Wire.endTransmission(false);
  Wire.requestFrom(0x68, 4, true); //Just 4 registers

  Gyr_rawX = Wire.read() << 8 | Wire.read(); //Once again we shif and sum
  Gyr_rawY = Wire.read() << 8 | Wire.read();


  /*---X---*/
  Gyro_angle[0] = Gyr_rawX / 131.0;
  /*---Y---*/
  Gyro_angle[1] = Gyr_rawY / 131.0;



  /*---X axis angle---*/
  Total_angle[0] = 0.98 * (Total_angle[0] + Gyro_angle[0] * elapsedTime) + 0.02 * Acceleration_angle[0];
  /*---Y axis angle---*/
  Total_angle[1] = 0.98 * (Total_angle[1] + Gyro_angle[1] * elapsedTime) + 0.02 * Acceleration_angle[1];


  // Serial.println(Total_angle[1]);



  /*///////////////////////////P I D///////////////////////////////////*/



  error = Total_angle[1] - desired_angle;



  pid_p = kp * error;


  if (-3 < error < 3)
  {
    pid_i = pid_i + (ki * error);
  }


  pid_d = kd * ((error - previous_error) / elapsedTime);


  PID = pid_p + pid_i + pid_d;

  // Serial.println('pid_p');

  //  Serial.println('pid_i');

  // Serial.println('PID');


  if (PID < -1000)
  {
    PID = -1000;
  }
  if (PID > 1000)
  {
    PID = 1000;
  }

  vel = map(PID, -1000, 1000, -400, 400);

  md.setM2Speed(vel);
  md.setM1Speed(vel);




}//end of loop void