Bajar el programa sigue líneas que lleva el mbot

Hemos programado el sensor sigue líneas pero no funciona tan bien como el que lleva por defecto el robot.

Intuímos que es más complejo, que guarda el ultimo valor leído, que las líneas discontinuas dibujadas en el circuito en forma de 8 tienen algún significado...

Conclusión: necesitaríamos ese programa en MBlock.
O que alguien nos explicara cómo hacerlo al revés, es decir, bajar el arduino de la placa a programa Mblock, código.

Muchas gracias a todos!!!!

Se supone que todo el entorno Makeblock sigue la filosofía "open" y comparte todo.

Aunque es un poco largo te dejo aquí el programa publicado por ellos, pero si buscas en repositorios open encontrarás más o menos lo mismo.

Busca "modeC" para la parte del sigue líneas.

Saludos!

Dani S.

#include <Servo.h>
#include <Wire.h>
#include <SoftwareSerial.h>
#include "mCore.h"
MeRGBLed rgb;
MeUltrasonic ultr(PORT_3);
MeLineFollower line(PORT_2);
MeLEDMatrix ledMx;
MeIR ir;
MeBuzzer buzzer;
MeTemperature ts;
Me7SegmentDisplay seg;

MeDCMotor MotorL(M1);
MeDCMotor MotorR(M2);
MePort generalDevice;
Servo servo;

int moveSpeed = 200;
int minSpeed = 48;
int factor = 23;

#define NTD1 294
#define NTD2 330
#define NTD3 350
#define NTD4 393
#define NTD5 441
#define NTD6 495
#define NTD7 556
#define NTDL1 147
#define NTDL2 165
#define NTDL3 175
#define NTDL4 196
#define NTDL5 221
#define NTDL6 248
#define NTDL7 278
#define NTDH1 589
#define NTDH2 661
#define NTDH3 700
#define NTDH4 786
#define NTDH5 882
#define NTDH6 990
#define NTDH7 112


#define RUN_F 0x01
#define RUN_B 0x01<<1
#define RUN_L 0x01<<2
#define RUN_R 0x01<<3
#define STOP 0
uint8_t motor_sta = STOP;
enum
{
  MODE_A,
  MODE_B,
  MODE_C
};

typedef struct MeModule
{
    int device;
    int port;
    int slot;
    int pin;
    int index;
    float values[3];
} MeModule;

union{
    byte byteVal[4];
    float floatVal;
    long longVal;
}val;

union{
  byte byteVal[8];
  double doubleVal;
}valDouble;

union{
  byte byteVal[2];
  short shortVal;
}valShort;

MeModule modules[12];
int analogs[8]={A0,A1,A2,A3,A4,A5,A6,A7};
uint8_t mode = MODE_A;

boolean isAvailable = false;
int len = 52;
char buffer[52];
char bufferBt[52];
byte index = 0;
byte dataLen;
byte modulesLen=0;
boolean isStart = false;
char serialRead;
String mVersion = "1.2.103";
float angleServo = 90.0;
unsigned char prevc=0;
boolean buttonPressed = false;
double lastTime = 0.0;
double currentTime = 0.0;
int LineFollowFlag=0;

#define VERSION 0
#define ULTRASONIC_SENSOR 1
#define TEMPERATURE_SENSOR 2
#define LIGHT_SENSOR 3
#define POTENTIONMETER 4
#define JOYSTICK 5
#define GYRO 6
#define SOUND_SENSOR 7
#define RGBLED 8
#define SEVSEG 9
#define MOTOR 10
#define SERVO 11
#define ENCODER 12
#define IR 13
#define IRREMOTE 14
#define PIRMOTION 15
#define INFRARED 16
#define LINEFOLLOWER 17
#define IRREMOTECODE 18
#define SHUTTER 20
#define LIMITSWITCH 21
#define BUTTON 22
#define DIGITAL 30
#define ANALOG 31
#define PWM 32
#define SERVO_PIN 33
#define TONE 34
#define BUTTON_INNER 35
#define LEDMATRIX 41
#define TIMER 50

#define GET 1
#define RUN 2
#define RESET 4
#define START 5


void setup()
{
  Stop();
  rgb.setNumber(16);
  rgb.clear();
  rgb.setColor(10, 0, 0);
  buzzer.tone(NTD1, 300); 
  delay(300);
  rgb.setColor(0, 10, 0);
  buzzer.tone(NTD2, 300);
  delay(300);
  rgb.setColor(0, 0, 10);
  buzzer.tone(NTD3, 300);
  delay(300);
  rgb.clear();
  Serial.begin(115200);
  buzzer.noTone();
  ir.begin(); 
}
unsigned char readBuffer(int index){
 return buffer[index]; 
}
void writeBuffer(int index,unsigned char c){
  buffer[index]=c;
}
void writeHead(){
  writeSerial(0xff);
  writeSerial(0x55);
}
void writeEnd(){
 Serial.println(); 
}
void writeSerial(unsigned char c){
 Serial.write(c);
}
void readSerial(){
  isAvailable = false;
  if(Serial.available()>0){
    isAvailable = true;
    serialRead = Serial.read();
  }
}
void serialHandle(){
  readSerial();
  if(isAvailable){
    unsigned char c = serialRead&0xff;
    if(c==0x55&&isStart==false){
     if(prevc==0xff){
      index=1; 
      isStart = true;
     }
    }else{
      prevc = c;
      if(isStart){
        if(index==2){
         dataLen = c; 
        }else if(index>2){
          dataLen--;
        }
        writeBuffer(index,c);
      }
    }
     index++;
     if(index>51){
      index=0; 
      isStart=false;
     }
     if(isStart&&dataLen==0&&index>3){ 
        isStart = false;
        parseData(); 
        index=0;
     }
  }
}
            int px = 0;
void loop()
{
  while(1)
  {
    get_ir_command();
    serialHandle();
    switch (mode)
    {
      case MODE_A:
        modeA();
        break;
      case MODE_B:
        modeB();
        break;
      case MODE_C:
        modeC();
        break;
    }
  }
}

void get_ir_command()
{
  static long time = millis();
  if (ir.decode())
  {
    uint32_t value = ir.value;
    time = millis();
    switch (value >> 16 & 0xff)
    {
      case IR_BUTTON_A:
        moveSpeed = 220;
        mode = MODE_A;
        Stop();
        buzzer.tone(NTD1, 300); 
        rgb.clear();
        rgb.setColor(10, 10, 10);
        break;
      case IR_BUTTON_B:
        moveSpeed = 200;
        mode = MODE_B;
        Stop();
        buzzer.tone(NTD2, 300); 
        rgb.clear();
        rgb.setColor(0, 10, 0);
        break;
      case IR_BUTTON_C:
        mode = MODE_C;
        moveSpeed = 120;
        Stop();
        buzzer.tone(NTD3, 300); 
        rgb.clear();
        rgb.setColor(0, 0, 10);
        break;
      case IR_BUTTON_PLUS:
        motor_sta = RUN_F;
        //buzzer.tone(NTD4, 300); 
        rgb.clear();
        rgb.setColor(10, 10, 0);
        //               Forward();
        break;
      case IR_BUTTON_MINUS:
        motor_sta = RUN_B;
        rgb.clear();
        rgb.setColor(10, 0, 0);
        //buzzer.tone(NTD4, 300); 
        //               Backward();
        break;
      case IR_BUTTON_NEXT:
        motor_sta = RUN_R;
        //buzzer.tone(NTD4, 300); 
        rgb.clear();
        rgb.setColor(1,10, 10, 0);
        //               TurnRight();
        break;
      case IR_BUTTON_PREVIOUS:
        motor_sta = RUN_L;
        //buzzer.tone(NTD4, 300); 
        rgb.clear();
        rgb.setColor(2,10, 10, 0);
        //               TurnLeft();
        break;
      case IR_BUTTON_9:
        buzzer.tone(NTDH2, 300); 
        delay(300);
        ChangeSpeed(factor * 9 + minSpeed);
        break;
      case IR_BUTTON_8:
        buzzer.tone(NTDH1, 300); 
        delay(300);
        ChangeSpeed(factor * 8 + minSpeed);
        break;
      case IR_BUTTON_7:
        buzzer.tone(NTD7, 300); 
        delay(300);
        ChangeSpeed(factor * 7 + minSpeed);
        break;
      case IR_BUTTON_6:
        buzzer.tone(NTD6, 300); 
        delay(300);
        ChangeSpeed(factor * 6 + minSpeed);
        break;
      case IR_BUTTON_5:
        buzzer.tone(NTD5, 300); 
        delay(300);
        ChangeSpeed(factor * 5 + minSpeed);
        break;
      case IR_BUTTON_4:
        buzzer.tone(NTD4, 300); 
        delay(300);
        ChangeSpeed(factor * 4 + minSpeed);
        break;
      case IR_BUTTON_3:
        buzzer.tone(NTD3, 300); 
        delay(300);
        ChangeSpeed(factor * 3 + minSpeed);
        break;
      case IR_BUTTON_2:
        buzzer.tone(NTD2, 300); 
        delay(300);
        ChangeSpeed(factor * 2 + minSpeed);
        break;
      case IR_BUTTON_1:
        buzzer.tone(NTD1, 300); 
        delay(300);
        ChangeSpeed(factor * 1 + minSpeed);
        break;
    }
  }
  else if (millis() - time > 120)
  {
    motor_sta = STOP;
    time = millis();
  }
}
void Forward()
{
  MotorL.run(-moveSpeed);
  MotorR.run(moveSpeed);
}
void Backward()
{
  MotorL.run(moveSpeed); 
  MotorR.run(-moveSpeed);
}
void TurnLeft()
{
  MotorL.run(-moveSpeed/10);
  MotorR.run(moveSpeed);
}
void TurnRight()
{
  MotorL.run(-moveSpeed);
  MotorR.run(moveSpeed/10);
}
void Stop()
{
  rgb.clear();
  MotorL.run(0);
  MotorR.run(0);
}
uint8_t prevState = 0;
void ChangeSpeed(int spd)
{
  buzzer.tone(NTD5, 300); 
  moveSpeed = spd;
}

void modeA()
{
  switch (motor_sta)
  {
    case RUN_F:
      Forward();
      prevState = motor_sta;
      break;
    case RUN_B:
      Backward();
      prevState = motor_sta;
      break;
    case RUN_L:
      TurnLeft();
      prevState = motor_sta;
      break;
    case RUN_R:
      TurnRight();
      prevState = motor_sta;
      break;
    case STOP:
      if(prevState!=motor_sta){
        prevState = motor_sta;
        Stop();
      }
      break;
  }

}

void modeB()
{
  uint8_t d = ultr.distanceCm(50);
  static long time = millis();
  randomSeed(analogRead(6));
  uint8_t randNumber = random(2);
  if (d > 15 || d == 0)Forward();
  else if (d > 10) {
    switch (randNumber)
    {
      case 0:
        TurnLeft();
        delay(200);
        break;
      case 1:
        TurnRight();
        delay(200);
        break;
    }
  }
  else
  {
    Backward();
    delay(400);
  }
  delay(100);
}

void modeC()
{
  uint8_t val;
  val = line.readSensors();
  if(moveSpeed >230)moveSpeed=230;
  switch (val)
  {
    case S1_IN_S2_IN:
      Forward();
      LineFollowFlag=10;
      break;

    case S1_IN_S2_OUT:
       Forward();
      if (LineFollowFlag>1) LineFollowFlag--;
      break;

    case S1_OUT_S2_IN:
      Forward();
      if (LineFollowFlag<20) LineFollowFlag++;
      break;

    case S1_OUT_S2_OUT:
      if(LineFollowFlag==10) Backward();
      if(LineFollowFlag<10) TurnLeft();
      if(LineFollowFlag>10) TurnRight();
      break;
  }
//  delay(50);
}
void parseData(){
  isStart = false;
  int idx = readBuffer(3);
  int action = readBuffer(4);
  int device = readBuffer(5);
  switch(action){
    case GET:{
        writeHead();
        writeSerial(idx);
        readSensor(device);
        writeEnd();
     }
     break;
     case RUN:{
       runModule(device);
       callOK();
     }
      break;
      case RESET:{
        //reset
        callOK();
      }
     break;
     case START:{
        //start
        callOK();
      }
     break;
  }
}
void callOK(){
    writeSerial(0xff);
    writeSerial(0x55);
    writeEnd();
}
void sendByte(char c){
  writeSerial(1);
  writeSerial(c);
}
void sendString(String s){
  int l = s.length();
  writeSerial(4);
  writeSerial(l);
  for(int i=0;i<l;i++){
    writeSerial(s.charAt(i));
  }
}
//1 byte 2 float 3 short 4 len+string 5 double
void sendFloat(float value){ 
     writeSerial(2);
     val.floatVal = value;
     writeSerial(val.byteVal[0]);
     writeSerial(val.byteVal[1]);
     writeSerial(val.byteVal[2]);
     writeSerial(val.byteVal[3]);
}
void sendShort(double value){
     writeSerial(3);
     valShort.shortVal = value;
     writeSerial(valShort.byteVal[0]);
     writeSerial(valShort.byteVal[1]);
     writeSerial(valShort.byteVal[2]);
     writeSerial(valShort.byteVal[3]);
}
void sendDouble(double value){
     writeSerial(5);
     valDouble.doubleVal = value;
     writeSerial(valDouble.byteVal[0]);
     writeSerial(valDouble.byteVal[1]);
     writeSerial(valDouble.byteVal[2]);
     writeSerial(valDouble.byteVal[3]);
     writeSerial(valDouble.byteVal[4]);
     writeSerial(valDouble.byteVal[5]);
     writeSerial(valDouble.byteVal[6]);
     writeSerial(valDouble.byteVal[7]);
}
short readShort(int idx){
  valShort.byteVal[0] = readBuffer(idx);
  valShort.byteVal[1] = readBuffer(idx+1);
  return valShort.shortVal; 
}
float readFloat(int idx){
  val.byteVal[0] = readBuffer(idx);
  val.byteVal[1] = readBuffer(idx+1);
  val.byteVal[2] = readBuffer(idx+2);
  val.byteVal[3] = readBuffer(idx+3);
  return val.floatVal;
}
char _receiveStr[20] = {};
uint8_t _receiveUint8[16] = {};
char* readString(int idx,int len){
  for(int i=0;i<len;i++){
    _receiveStr[i]=readBuffer(idx+i);
  }
  _receiveStr[len] = '\0';
  return _receiveStr;
}
uint8_t* readUint8(int idx,int len){
  for(int i=0;i<len;i++){
    if(i>15){
      break;
    }
    _receiveUint8[i] = readBuffer(idx+i);
  }
  return _receiveUint8;
}
void runModule(int device){
  //0xff 0x55 0x6 0x0 0x2 0x22 0x9 0x0 0x0 0xa 
  int port = readBuffer(6);
  int pin = port;
  switch(device){
   case MOTOR:{
     int speed = readShort(7);
     port==M1?MotorL.run(speed):MotorR.run(speed);
   } 
    break;
    case JOYSTICK:{
     int leftSpeed = readShort(6);
     MotorL.run(leftSpeed);
     int rightSpeed = readShort(8);
     MotorR.run(rightSpeed);
    }
    break;
   case RGBLED:{
     int idx = readBuffer(7);
     int r = readBuffer(8);
     int g = readBuffer(9);
     int b = readBuffer(10);
     rgb.setColor(idx,r, g, b);
     rgb.show();
   }
   break;
   case SERVO:{
     int slot = readBuffer(7);
     pin = slot==1?mePort[port].s1:mePort[port].s2;
     int v = readBuffer(8);
     if(v>=0&&v<=180){
       servo.attach(pin);
       servo.write(v);
     }
   }
   break;
   case SEVSEG:{
     if(seg.getPort()!=port){
       seg.reset(port);
     }
     float v = readFloat(7);
     seg.display(v);
   }
   break;
   case LEDMATRIX:{
     if(ledMx.getPort()!=port){
       ledMx.reset(port);
     }
     int action = readBuffer(7);
     if(action==1){
            int brightness = readBuffer(8);
            int color = readBuffer(9);
            int px = readShort(10);
            int py = readShort(12);
            int len = readBuffer(14);
            char *s = readString(15,len);
            ledMx.clearScreen();
            ledMx.setColorIndex(color);
            ledMx.setBrightness(brightness);
            ledMx.drawStr(px,py,s);
         }else if(action==2){
           int brightness = readBuffer(8);
            int dw = readBuffer(9);
            int px = readShort(10);
            int py = readShort(12);
            int len = readBuffer(14);
            uint8_t *ss = readUint8(15,len);
            ledMx.clearScreen();
            ledMx.setColorIndex(1);
            ledMx.setBrightness(brightness);
            ledMx.drawBitmap(px,py,dw,ss);
         }else if(action==3){
            int brightness = readBuffer(8);
            int point = readBuffer(9);
            int hours = readShort(10);
            int minutes = readShort(12);
            ledMx.clearScreen();
            ledMx.setColorIndex(1);
            ledMx.setBrightness(brightness);
            ledMx.showClock(hours,minutes,point);
     }
   }
   break;
   case LIGHT_SENSOR:{
     if(generalDevice.getPort()!=port){
       generalDevice.reset(port);
     }
     int v = readBuffer(7);
     generalDevice.dWrite1(v);
   }
   break;
   case IR:{
     int len = readBuffer(2)-3;
     String s ="";
     for(int i=0;i<len;i++){
       s+=(char)readBuffer(6+i);
     }
     ir.sendString(s);
   }
   break;
   case SHUTTER:{
     if(generalDevice.getPort()!=port){
       generalDevice.reset(port);
     }
     int v = readBuffer(7);
     if(v<2){
       generalDevice.dWrite1(v);
     }else{
       generalDevice.dWrite2(v-2);
     }
   }
   break;
   case DIGITAL:{
     pinMode(pin,OUTPUT);
     int v = readBuffer(7);
     digitalWrite(pin,v);
   }
   break;
   case PWM:{
     pinMode(pin,OUTPUT);
     int v = readBuffer(7);
     analogWrite(pin,v);
   }
   break;
   case TONE:{
     pinMode(pin,OUTPUT);
     int hz = readShort(6);
     if(hz>0){
       buzzer.tone(hz); 
     }else{
       buzzer.noTone(); 
     }
   }
   break;
   case SERVO_PIN:{
     int v = readBuffer(7);
     if(v>=0&&v<=180){
       servo.attach(pin);
       servo.write(v);
     }
   }
   break;
   case TIMER:{
    lastTime = millis()/1000.0; 
   }
   break;
  }
}
void readSensor(int device){
  /**************************************************
      ff    55      len idx action device port slot data a
      0     1       2   3   4      5      6    7    8
      0xff  0x55   0x4 0x3 0x1    0x1    0x1  0xa 
  ***************************************************/
  float value=0.0;
  int port,slot,pin;
  port = readBuffer(6);
  pin = port;
  switch(device){
   case  ULTRASONIC_SENSOR:{
     if(ultr.getPort()!=port){
       ultr.reset(port);
     }
     value = (float)ultr.distanceCm(50000);
     sendFloat(value);
   }
   break;
   case  TEMPERATURE_SENSOR:{
     slot = readBuffer(7);
     if(ts.getPort()!=port||ts.getSlot()!=slot){
       ts.reset(port,slot);
     }
     value = ts.temperature();
     sendFloat(value);
   }
   break;
   case  LIGHT_SENSOR:
   case  SOUND_SENSOR:
   case  POTENTIONMETER:{
     if(generalDevice.getPort()!=port){
       generalDevice.reset(port);
       pinMode(generalDevice.pin2(),INPUT);
     }
     value = generalDevice.aRead2();
     sendFloat(value);
   }
   break;
   case  JOYSTICK:{
     slot = readBuffer(7);
     if(generalDevice.getPort()!=port){
       generalDevice.reset(port);
       pinMode(generalDevice.pin1(),INPUT);
       pinMode(generalDevice.pin2(),INPUT);
     }
     if(slot==1){
       value = generalDevice.aRead1();
       sendFloat(value);
     }else if(slot==2){
       value = generalDevice.aRead2();
       sendFloat(value);
     }
   }
   break;
   case  IR:{
//     if(ir.getPort()!=port){
//       ir.reset(port);
//     }
//      if(irReady){
//         sendString(irBuffer);
//         irReady = false;
//         irBuffer = "";
//      }
   }
   break;
   case IRREMOTE:{
//     unsigned char r = readBuffer(7);
//     if(millis()/1000.0-lastIRTime>0.2){
//       sendByte(0);
//     }else{
//       sendByte(irRead==r);
//     }
//     //irRead = 0;
//     irIndex = 0;
   }
   break;
   case IRREMOTECODE:{
//     sendByte(irRead);
//     irRead = 0;
//     irIndex = 0;
   }
   break;
   case  PIRMOTION:{
     if(generalDevice.getPort()!=port){
       generalDevice.reset(port);
       pinMode(generalDevice.pin2(),INPUT);
     }
     value = generalDevice.dRead2();
     sendFloat(value);
   }
   break;
   case  LINEFOLLOWER:{
     if(generalDevice.getPort()!=port){
       generalDevice.reset(port);
         pinMode(generalDevice.pin1(),INPUT);
         pinMode(generalDevice.pin2(),INPUT);
     }
     value = generalDevice.dRead1()*2+generalDevice.dRead2();
     sendFloat(value);
   }
   break;
   case LIMITSWITCH:{
     slot = readBuffer(7);
     if(generalDevice.getPort()!=port||generalDevice.getSlot()!=slot){
       generalDevice.reset(port,slot);
     }
     if(slot==1){
       pinMode(generalDevice.pin1(),INPUT_PULLUP);
       value = generalDevice.dRead1();
     }else{
       pinMode(generalDevice.pin2(),INPUT_PULLUP);
       value = generalDevice.dRead2();
     }
     sendFloat(value);  
   }
   break;
   case BUTTON_INNER:{
     pin = analogs[pin];
     char s = readBuffer(7);
     pinMode(pin,INPUT);
     boolean currentPressed = !(analogRead(pin)>10);
     sendByte(s^(currentPressed?1:0));
     buttonPressed = currentPressed;
   }
   break;
   case  GYRO:{
//       int axis = readBuffer(7);
//       gyro.update();
//       if(axis==1){
//         value = gyro.getAngleX();
//         sendFloat(value);
//       }else if(axis==2){
//         value = gyro.getAngleY();
//         sendFloat(value);
//       }else if(axis==3){
//         value = gyro.getAngleZ();
//         sendFloat(value);
//       }
   }
   break;
   case  VERSION:{
     sendString(mVersion);
   }
   break;
   case  DIGITAL:{
     pinMode(pin,INPUT);
     sendFloat(digitalRead(pin));
   }
   break;
   case  ANALOG:{
     pin = analogs[pin];
     pinMode(pin,INPUT);
     sendFloat(analogRead(pin));
   }
   break;
   case TIMER:{
     sendFloat(currentTime);
   }
   break;
  }
}