Clumsee 101

Clumsee es un robot que a medida que se va añadiendo módulo por módulo se va haciendo cada vez más grande y nos puede dar problemas asociados con la memoria.

Por ello, existe una alternativa estable de este Robot para llevarla a cabo con una placa Arduino 101.

Esta placa, nos da una mayor estabilidad con sus sistema de navegación CurieIMU y además su conexión Bluetooth CurieBLE nos permite ahorrar energía y favorece el flujo de comunicación remota.

Para comenzar debemos realizar una calibración de sensores de color para poder manejar de manera robusta este modo de funcionamiento. En el siguiente enlace se muestra cómo.


 

#include <EEPROM.h>
#include <CurieBLE.h>
#include <CurieIMU.h>
#include <MadgwickAHRS.h>
#include <Servo.h>
#define SERIAL_DEBUG
#define DEBUG_CAL 1
#include <TCS3200.h>

//IMU Declarations and variables
#define NORTH 0
#define EAST 90
#define WEST 270
#define SOUTH 180

#define VMAX 100

Madgwick filter;
unsigned long microsPerReading, microsPrevious;
float accelScale, gyroScale;

float roll, pitch, yaw, new_yaw;

int angle = 180;
int angle_threshold = 15;
int right_bound = angle-angle_threshold;
int left_bound = angle+angle_threshold;
int ref = 0;

//Movement declarations

int LeftServoPin =5;
int RightServoPin=6;

Servo LeftServo;
Servo RightServo;

String actual_move = "FORWARD";
String last_move = "LEFT";

//BLE Service and Characteristic Declarations 
BLEPeripheral ble; // create peripheral instance

BLEService clumseeService("19B10010-E8F2-537E-4F6C-D104768A1214"); // create service
BLECharacteristic clumseeSTR("19B10011-E8F2-537E-4F6C-D104768A1215", BLERead | BLEWrite| BLENotify,16);

char LocalString[32];
char IMUString[32];

//TCS3200 Backward , forward and Color Function Matrix Declaration
TCS3200 CS_F(2,3,4);
TCS3200 CS_B(8,9,10);

typedef void (*matrixFunction) ();
 
#define MAXFROWS 8
#define MAXFCOLS 8

void f(){}
matrixFunction mF[MAXFROWS][MAXFCOLS]={
  {f,f,f,f,f,f,f,f},
  {f,f,f,f,f,f,f,f},
  {f,f,f,f,f,f,f,f},
  {f,f,f,f,f,f,f,f},
  {f,f,f,f,f,f,f,f},
  {f,f,f,f,f,f,f,f},
  {f,f,f,f,f,f,f,f},
  {f,f,f,f,f,f,f,f}
};

//Servo Velocity Control Functions
void forward(int v = VMAX){
  attachWheels(LeftServoPin, RightServoPin);
  LeftServo.write(map(v, 0,100,90,0));
  RightServo.write(map(v, 0,100,90,180));
}

void backward(int v = VMAX){
  attachWheels(LeftServoPin, RightServoPin);
  LeftServo.write(map(v, 0,100,90,180));
  RightServo.write(map(v, 0,100,90,0));
}

void left(int v = VMAX){
  attachWheels(LeftServoPin, RightServoPin);
  LeftServo.write(map(v, 0,100,90,180));
  RightServo.write(map(v, 0,100,90,180));
}

void right(int v = VMAX){
  attachWheels(LeftServoPin, RightServoPin);
  LeftServo.write(map(v, 0,100,90,0));
  RightServo.write(map(v, 0,100,90,0));
}

void stop(){
  Serial.println("Stop ");
  LeftServo.write(90);
  RightServo.write(90);
  detachWheels();
}

void attachWheels(int pinLeft, int pinRight){
  RightServo.attach(pinRight);
  LeftServo.attach(pinLeft);
  
}

void detachWheels(){
  RightServo.detach();
  LeftServo.detach();
}

void setup() {
  Serial.begin(38400);
  Serial.println("CLUMSEE101");

  //Servo Init
  pinMode(LeftServoPin, OUTPUT); 
  pinMode(RightServoPin, OUTPUT);
  detachWheels();
  delay(2000);
  
  // start the IMU and filter Definition Calibration
  CurieIMU.begin();
  CurieIMU.setGyroRate(25);
  CurieIMU.setAccelerometerRate(25);
  filter.begin(25);
  // Set the accelerometer range to 2G
  CurieIMU.setAccelerometerRange(2);
  // Set the gyroscope range to 250 degrees/second
  CurieIMU.setGyroRange(250);

  CurieIMU.autoCalibrateGyroOffset();
  CurieIMU.autoCalibrateAccelerometerOffset(X_AXIS, 0);
  CurieIMU.autoCalibrateAccelerometerOffset(Y_AXIS, 0);
  CurieIMU.autoCalibrateAccelerometerOffset(Z_AXIS, 1);
  // initialize variables to pace updates to correct rate
  microsPerReading = 1000000 / 25;
  microsPrevious = micros();

  //Define BLE Initialization
  // set the local name peripheral advertises
  ble.setLocalName("CLUMSEE101");
  // set the UUID for the service this peripheral advertises
  ble.setAdvertisedServiceUuid(clumseeService.uuid());
  ble.setAppearance(128);

  // add service and characteristic for Clumsee Movement
  ble.addAttribute(clumseeService);
  ble.addAttribute(clumseeSTR);

  // assign event handlers for connected, disconnected to peripheral
  ble.setEventHandler(BLEConnected, blePeripheralConnectHandler);
  ble.setEventHandler(BLEDisconnected, blePeripheralDisconnectHandler);

  // assign event handlers for characteristic
  clumseeSTR.setEventHandler(BLEWritten, clumseeReadSTR);

  // Write into the Local Strings characteristic *
  sprintf(LocalString,"AZERTYUIOPQSDFGH");
  clumseeSTR.setValue((unsigned char *)LocalString,16) ;

  // Begin BLE
  ble.begin();

  //TCS3200 Init
  CS_F.begin();
  CS_F.nSamples(40);
  CS_F.setRefreshTime(200);
  CS_F.setFrequency(TCS3200_FREQ_HI);
  CS_F._nEEPROM = 0;
  CS_F.setID("CS_F");
  
  CS_B.begin();
  CS_B.nSamples(40);
  CS_B.setRefreshTime(200);
  CS_B.setFrequency(TCS3200_FREQ_HI);
  CS_B._nEEPROM = 200;
  CS_B.setID("CS_B");

  //Calibration TCS3200 Color Sensors
  //CS_F.calibration(0);
  CS_F.loadCal(0);
  //CS_B.calibration(200);
  CS_B.loadCal(200);

  //Adding Matrix Color Functions 
  mF[0][0] = mf00;
  mF[1][1] = mf11;
  mF[2][2] = mf22;
  mF[3][3] = mf33;
  mF[4][4] = mf44;
  mF[5][5] = mf55;
  mF[6][6] = mf66;
  mF[7][7] = mf77;
  Serial.println("Clumsee Initialization over... Are you ready");
  
}

void loop() {
  unsigned long microsNow;
  
  ble.poll();
  
  // check if it's time to read data and update the filter
  microsNow = micros();
  if (microsNow - microsPrevious >= microsPerReading) {
    IMU_update();
    yawmove();
  }

  //Update Movement Status
  checkMove();
  /*if (CS_F.onChangeColor()){
    Serial.println(CS_F._lastColor);
    colortable(CS_F._lastColor,1);
  }
  if (CS_B.onChangeColor()){
    Serial.println(CS_B._lastColor);
    colortable(0,0);
  }*/
  //Check actions depending on color Readings
  if (CS_F.onChangeColor() || CS_B.onChangeColor()){
    colortable(CS_F._lastColor, CS_B._lastColor);
  }
  
}

void blePeripheralConnectHandler(BLECentral& central) {
  // central connected event handler
  Serial.print("Connected event, central: ");
  Serial.println(central.address());
}

void blePeripheralDisconnectHandler(BLECentral& central) {
  // central disconnected event handler
  Serial.print("Disconnected event, central: ");
  Serial.println(central.address());
}

void clumseeReadSTR(BLECentral& central, BLECharacteristic& characteristic) {

  Serial.print("Characteristic with UUID ");
  Serial.print(characteristic.uuid());
  Serial.print(" with BLE Length ");
  Serial.println(characteristic.valueLength());
  sprintf(LocalString,"%16c",NULL);
  strncpy(LocalString,(char *)characteristic.value(),characteristic.valueLength());
  Serial.print("Message Poll ");
  Serial.println( LocalString );
  choose_direction(LocalString ,characteristic.valueLength() );

}

float convertRawAcceleration(int aRaw) {
  // since we are using 2G range
  // -2g maps to a raw value of -32768
  // +2g maps to a raw value of 32767
 
  float a = (aRaw * 2.0) / 32768.0;
  return a;
}

float convertRawGyro(int gRaw) {
  // since we are using 250 degrees/seconds range
  // -250 maps to a raw value of -32768
  // +250 maps to a raw value of 32767
 
  float g = (gRaw * 250.0) / 32768.0;
  return g;
}

void IMU_update(){
    int aix, aiy, aiz;
    int gix, giy, giz;
    float ax, ay, az;
    float gx, gy, gz;
    // read raw data from CurieIMU
    CurieIMU.readMotionSensor(aix, aiy, aiz, gix, giy, giz);

    // convert from raw data to gravity and degrees/second units
    ax = convertRawAcceleration(aix);
    ay = convertRawAcceleration(aiy);
    az = convertRawAcceleration(aiz);
    gx = convertRawGyro(gix);
    gy = convertRawGyro(giy);
    gz = convertRawGyro(giz);

    // update the filter, which computes orientation
    filter.updateIMU(gx, gy, gz, ax, ay, az);

    // print the heading, pitch and roll
    roll = filter.getRoll();
    pitch = filter.getPitch();
    yaw = filter.getYaw();
    Serial.print("Orientation: ");
    Serial.print(yaw);
    Serial.print(" ");
    Serial.print(pitch);
    Serial.print(" ");
    Serial.println(roll);

    // increment previous time, so we keep proper pace
    microsPrevious = microsPrevious + microsPerReading;
}

void yawmove(){
  if( yaw <360 && yaw > (360 - ref)){
    new_yaw = yaw -360 + ref;
  }else{
    new_yaw = yaw + ref;
  }
  
  if(new_yaw > (angle-angle_threshold) && new_yaw < (angle+angle_threshold) ){ actual_move="FORWARD"; }else if(new_yaw > (angle+angle_threshold)){
    actual_move="RIGHT";
  }else if(new_yaw < (angle-angle_threshold)){
    actual_move="LEFT";
  }
}

void checkMove(){
  if (last_move != actual_move){
    if(actual_move == "FORWARD"){
      //stop();
      forward(50);
      Serial.println("FORWARD");
    }else if(actual_move == "RIGHT"){
      right(10);
      Serial.println("RIGHT");
    }else if(actual_move == "LEFT"){
      left(25);
      Serial.println("LEFT");
    }
    last_move = actual_move;
  }
}

void choose_direction( String dataref, int sizestr ){
    String reference = dataref.substring(0,sizestr);
    if( reference == "NORTH"){
      ref = 0;
    }else if(reference == "WEST"){
      ref = 270;
    }else if(reference == "EAST"){
      ref = 90;
    }else if(reference == "SOUTH"){
      ref = 180;
    }
    Serial.print("New Reference with length ");
    Serial.print(reference.length());
    Serial.print("  ");
    Serial.println(reference);
}

void colortable(int color_F, int color_B){
  mF[color_F][color_B]();
}

//Both White Color
void mf00(){
  Serial.println("F00");
}

//Both Black Color
void mf11(){
  Serial.println("F11");
  ref = SOUTH;
}

//Both Yellow Color
void mf22(){
  Serial.println("F22");
  //ref = SOUTH;
}

//Both orange Color
void mf33(){
  Serial.println("F33");
}

//Both Red Color
void mf44(){
  Serial.println("F44");
  ref = WEST;
}

//Both Green Color
void mf55(){
  Serial.println("F55");
  ref = EAST;
}

//Both Blue Color
void mf66(){
  Serial.println("F66");
  ref = NORTH;
}

//Both Brown Color
void mf77(){
  Serial.println("F77");
}

En este ejemplo, solamente se dispone del funcionamiento de Clumsee en modo autónomo que responde solamente a las lecturas leidas por los sensores de color.

Ahora realizaremos un ejemplo híbrido con el que podemos comunicar a través de una aplicación con App Inventor con la extensión BLE.

 

 

Estos programas ocupan el 50% del espacio de almacenamiento en memoria. Y aún nos queda por incluir la dinámica de juego en función de los valores leidos por los sensores.