Voici le programme par défaut du Mbot (source officielle de Makeblock sur Github), Il n'y a pas de restrictions concernant la modification du code.
https://github.com/Makeblock-official/mBotEt pour avoir un apperçu, le code du .ino
#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;
}
}
