this is the code we are using:
‘#include Arduino.h
’#include Wire.h
’#include SoftwareSerial.h
’#include MeMCore.h
MeDCMotor motor_9(9);
MeDCMotor motor_10(10);
void move(int direction, int speed)
{
int leftSpeed = 0;
int rightSpeed = 0;
if(direction == 1){
leftSpeed = speed;
rightSpeed = speed;
}else if(direction == 2){
leftSpeed = -speed;
rightSpeed = -speed;
}else if(direction == 3){
leftSpeed = -speed;
rightSpeed = speed;
}else if(direction == 4){
leftSpeed = speed;
rightSpeed = -speed;
}
motor_9.run((9)==M1?-(leftSpeed):(leftSpeed));
motor_10.run((10)==M1?-(rightSpeed):(rightSpeed));
}
double angle_rad = PI/180.0;
double angle_deg = 180.0/PI;
void stoop();
void right();
void left();
void furword();
void backward();
double tiome;
Servo servo_4_1;
MePort port_4(4);
Servo servo_4_2;
void stoop()
{
motor_9.run((9)==M1?-(0):(0));
motor_10.run((10)==M1?-(0):(0));
}
void right()
{
motor_9.run((9)==M1?-(150):(150));
motor_10.run((10)==M1?-(0):(0));
}
void left()
{
motor_9.run((9)==M1?-(0):(0));
motor_10.run((10)==M1?-(150):(150));
}
void furword()
{
motor_9.run((9)==M1?-(150):(150));
motor_10.run((10)==M1?-(150):(150));
}
void backward()
{
motor_9.run((9)==M1?-(-150):(-150));
motor_10.run((10)==M1?-(-150):(-150));
}
void setup(){
servo_4_1.attach(port_4.pin1());
servo_4_2.attach(port_4.pin2());
tiome = 0.5;
_delay(15);
}
void loop(){
furword();
_delay(tiome);
stoop();
_delay(tiome);
servo_4_1.write(0);
servo_4_2.write(180);
furword();
_delay(tiome);
stoop();
_delay(tiome);
servo_4_1.write(45);
servo_4_2.write(135);
furword();
_delay(tiome);
stoop();
_delay(tiome);
servo_4_1.write(90);
servo_4_2.write(90);
furword();
_delay(tiome);
stoop();
_delay(tiome);
servo_4_1.write(135);
servo_4_2.write(45);
furword();
_delay(tiome);
stoop();
_delay(tiome);
servo_4_1.write(180);
servo_4_2.write(0);
right();
_delay(tiome);
left();
_delay(tiome);
right();
_delay(tiome);
stoop();
_delay(tiome);
servo_4_1.write(180);
servo_4_2.write(0);
right();
_delay(tiome);
left();
_delay(tiome);
right();
_delay(tiome);
stoop();
servo_4_1.write(180);
servo_4_2.write(0);
right();
_delay(tiome);
left();
_delay(tiome);
right();
_delay(tiome);
stoop();
_delay(tiome);
servo_4_1.write(180);
servo_4_2.write(0);
right();
_delay(tiome);
left();
_delay(tiome);
right();
_delay(tiome);
stoop();
_delay(tiome);
_delay(30);
_loop();
}
void _delay(float seconds){
long endTime = millis() + seconds * 1000;
while(millis() < endTime)_loop();
}
void _loop(){
}