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.