Project showcase
MAX009

MAX009 © GPL3+

MeArm mounted on smart robot car controlled by 2 Arduino UNOs

  • 15,938 views
  • 52 comments
  • 41 respects

Components and supplies

Apps and online services

About this project

Features

The MAX009 is a 4 degrees of freedom (4DOF) robotic arm mounted on a 2 wheel drive (2WD) smart robot car.

The smart robot car has 2 DC motors and 2 infrared speed sensors, allowing accurate speed regulation of the 2 wheels. 1 ultrasonic distance sensor and 2 infrared proximity sensors can be used for obstacle avoidance . It has head, rear, left and right led lights, horn, ...

The robotic arm is a MeArm with 2 joysticks for manual control and an extra white led on the base.

The MAX009 is controlled by 2 arduino's (one for the car, one for the arm) communicating between each other and with the external world via bluetooth. It is entirely remote controllable by a dedicated android app. It can be programmed via a simple LOGO interpreter embedded in one of the arduino's.

One logo program can be stored in the Arduino EEPROM and started by pressing a push button. 10 programs can be stored in the smartphone memory using the MAX009 android app. Both the smart robot car and the MeArm are powered by a single 9V Li-Ion battery.

Videos

MAX009 Remote control demo
MAX009 Manual control demo
MAX009 LOGO interpreter demo
MAX009 Introduction
MAX009 Collision avoidance demo

Android app

On the top section, the arrows control the smart robot car. The buttons stack controls the lights and the horn. In the middle part, the double set of arrows controls the MeArm robotic arm, just like the 2 joysticks on the Arduino shield.

At the bottom, LOGO commands can be entered and sent to the robot. The last command can be recalled by pushing the “Last” button. 10 commands (or list of commands) can be saved and recalled from the phone memory using the“Save” and “Load” buttons.

Finally, the robot car can be controlled by the phone gyro sensors by checking the “Use sensors” box.

Supported LOGO commands

  • fw (n) : move forward (by n steps)
  • bw (n) : move backward (by n steps)
  • tl (n) : turn left (by n steps)
  • tr (n) : turn right (by n steps)
  • st : stop move
  • pa [n] : pause of n milliseconds
  • hl [x] : head light [0: OFF, 1: ON]
  • ho [x] : horn [0: OFF, 1: ON]
  • lr [x] : left rear light [0: OFF, 1: ON]
  • rr [x] : right read light [0: OFF, 1: ON]
  • ll [x] : left light [0: OFF, 1: CLIGN, 2:ON]
  • rl [x] : right light [0: OFF, 1: CLIGN, 2:ON]
  • ss [n] : set speed
  • sp : save program
  • ep : end of program
  • rp : run program
  • pp : print program
  • matl : MeArm turn left
  • matr : MeArm turn right
  • mafw : MeArm move forward
  • mabw : MeArm move backward
  • maup : MeArm move up
  • mado : MeArm move down
  • maop : MeArm open claw
  • macl : MeArm close claw
  • mast : MeArm stop move
  • maip : MeArm reset to initial position
  • marp : MeArm run demo program
  • mas1 [x] : MeArm move servo 1 to position x
  • mas2 [x] : MeArm move servo 2 to position x
  • mas3 [x] : MeArm move servo 3 to position x
  • mas4 [x] : MeArm move servo 4 to position x
  • mali [x] : MeArm set light [0: OFF, 1: ON]
  • repeat [n] [cmd] : repeat "cmd" n times
  • he : help

Bill of materials

  • 2 arduino UNO's
  • 1 robot car chassis
  • 2 DC motors
  • 1 Dual L9110 motor driver
  • 2 infrared speed sensors
  • 1 dual channel motor driver
  • 1 HC-SR04 ultrasonic distance meter with holder
  • 1 ULN2003N
  • 1 HC-06 Bluetooth receiver
  • 1 Buzzer (horn)
  • 1 9V (8.2V) Li-Ion battery (like soshine 650) with holder
  • 1 Push button
  • 1 MeArm kit
  • 4 micro servos (3 Tower Pro MG90S + 1 SG90 for the claw)
  • 2-joysticks arduino shield (homemade)
  • Red and white leds, resistors, screws, …

Final hints

Feel free to buy any low cost kit for the robot car chassis and the MeArm mechanical parts, but put the money in good motors.

For the robotic arm, excepted for the claw, it is mandatory to choose metal bearing servos with analogue controller, otherwise the arm can become shaky or may even oscillate.

For the robot car, stepper motors could provide more accurate moves compared to the DC motors used in this project.

Code

MAX009 arduino code for smart robot carArduino
Code to be uploaded in the arduino UNO mounted on the smart robot car
#include <MsTimer2.h>
#include <EEPROM.h>
#include <SoftwareSerial.h>

const int IntPin0=2;
const int IntPin1=3;
const int IntDebounce=50;
const int MotorRDirectionPin =  4; 
const int MotorRSpeedPin =  5; 
const int MotorLDirectionPin =  7; 
const int MotorLSpeedPin =  6;
const int ProxSensor1Pin = 8;
const int ProxSensor2Pin = 9;
const int EchoTriggerPin = 10;
const int EchoPin = 11;
SoftwareSerial softSerial(100,12); // RX, TX
const int ButtonPin = 13;
const int HeadLightsPin = A0;
const int HornPin = A1;
const int LeftLightPin = A2;
const int leftRearLightPin = A3;
const int rightRearLightPin = A4;
const int RightLightPin = A5;
const int maxLineBuffer=80;
double Kc=0.1;
unsigned int speedTarget=150;
const int rotPwm=130;
unsigned int dist=0;

char c;
String lineBuffer;
String pgrmString="";
int inputMode=0; // 0 = normal/interactive, 1 = record program
double pwmInit=130;
double pwmL=pwmInit;
double lasterrorL=0;
double pwmR=pwmInit;
double lasterrorR=0;
int stepCntTarget=0;
boolean useProxSensor=0;

volatile int PIDstate=0;  // 0 = stop, 1 = forward, 2 = backward
volatile unsigned long prevIntTimeL;
volatile unsigned long intPeriodL;
volatile unsigned long prevIntTimeR;
volatile unsigned long intPeriodR;
volatile unsigned int stepCntL=0;
volatile unsigned int stepCntR=0;
unsigned long PIDi=0;

boolean LeftLightState=false;
boolean RightLightState=false;
int SideLightCnt=0;

void setup() {
  pinMode(MotorRDirectionPin, OUTPUT);
  pinMode(MotorRSpeedPin, OUTPUT);
  pinMode(MotorLDirectionPin, OUTPUT);
  pinMode(MotorLSpeedPin, OUTPUT);
  digitalWrite(MotorRDirectionPin,LOW);
  digitalWrite(MotorRSpeedPin,LOW);
  digitalWrite(MotorLDirectionPin,LOW);
  digitalWrite(MotorLSpeedPin,LOW);
  pinMode(ButtonPin, INPUT);
  digitalWrite(ButtonPin, HIGH);
  pinMode(IntPin0,INPUT);
  digitalWrite(IntPin0,HIGH);
  pinMode(IntPin1,INPUT);
  digitalWrite(IntPin1,HIGH);
  attachInterrupt(0, ISR0, RISING);
  attachInterrupt(1, ISR1, RISING);
  pinMode(ProxSensor1Pin,INPUT);
  digitalWrite(ProxSensor1Pin,HIGH);  
  pinMode(ProxSensor2Pin,INPUT);
  digitalWrite(ProxSensor2Pin,HIGH);  
  pinMode(EchoPin,INPUT);
  pinMode(EchoTriggerPin,OUTPUT);
  digitalWrite(EchoTriggerPin,LOW);
  pinMode(HeadLightsPin,OUTPUT);
  digitalWrite(HeadLightsPin,LOW);
  pinMode(HornPin,OUTPUT);
  digitalWrite(HornPin,LOW);
  pinMode(LeftLightPin,OUTPUT);
  digitalWrite(LeftLightPin,LOW);
  pinMode(RightLightPin,OUTPUT);
  digitalWrite(RightLightPin,LOW);
  pinMode(leftRearLightPin,OUTPUT);
  digitalWrite(leftRearLightPin,LOW);
  pinMode(rightRearLightPin,OUTPUT);
  digitalWrite(rightRearLightPin,LOW);
    
  if (digitalRead(ProxSensor1Pin)==LOW || digitalRead(ProxSensor2Pin)==LOW) {
    useProxSensor=false;
    horn(1);
    delay(100);
    horn(0);
  } else {
    useProxSensor=true;
    horn(1);
    delay(100);
    horn(0);
    delay(100);
    horn(1);
    delay(100);
    horn(0);
  }

  Serial.begin(115200);
  // Load program from eeprom
  int i=0;
  c=EEPROM.read(i);
  while(c!=0) {
    pgrmString+=c;
    i++;
    c=EEPROM.read(i);
  }

  softSerial.begin(9600);
  
  // Timer interrupt
  MsTimer2::set(50, timerISR); 
  MsTimer2::start();
}

void loop() {
  while (Serial.available() > 0) {
    c = Serial.read();
    if (c=='\n' || lineBuffer.length()==maxLineBuffer) {
      if (inputMode==1) {
        lineBuffer.trim();
        if (lineBuffer.compareTo("ep")==0) {
          // Save program to eeprom
          for (int i=0;i<pgrmString.length();i++) EEPROM.write(i,pgrmString.charAt(i));
          EEPROM.write(pgrmString.length(),0);
          Serial.println("Program recorded : ");
          Serial.println(pgrmString);
          inputMode=0;
        } else {  
          if (!lineBuffer.endsWith(";") && !lineBuffer.endsWith("]")) lineBuffer+=';';
          pgrmString=pgrmString+lineBuffer;
        }
      } else execPgrm(lineBuffer);
      lineBuffer="";
    }
    else lineBuffer+=c;
  }
  if (digitalRead(ButtonPin)==LOW) execPgrm(pgrmString);
}

void moveForward() {
  PIDstate=1;
  PIDi=0;
  pwmL=pwmInit;
  pwmR=pwmInit;
  digitalWrite(MotorLDirectionPin,LOW);
  analogWrite(MotorLSpeedPin,pwmL);    
  digitalWrite(MotorRDirectionPin,LOW);
  analogWrite(MotorRSpeedPin,pwmR);  
}

void moveBackward() {
  PIDstate=2;
  PIDi=0;
  digitalWrite(MotorLDirectionPin,HIGH);
  analogWrite(MotorLSpeedPin,255-pwmL);
  digitalWrite(MotorRDirectionPin,HIGH);
  analogWrite(MotorRSpeedPin,255-pwmR);
}

void turnRight() { 
  digitalWrite(MotorLDirectionPin,LOW);
  analogWrite(MotorLSpeedPin,rotPwm);
  digitalWrite(MotorRDirectionPin,HIGH);
  analogWrite(MotorRSpeedPin,255-rotPwm);
}

void turnLeft() {
  digitalWrite(MotorRDirectionPin,LOW);
  analogWrite(MotorRSpeedPin,rotPwm);
  digitalWrite(MotorLDirectionPin,HIGH);
  analogWrite(MotorLSpeedPin,255-rotPwm);
}

void stopMove() {
  PIDstate=0;
  digitalWrite(MotorRDirectionPin,LOW);
  digitalWrite(MotorRSpeedPin,LOW);
  digitalWrite(MotorLDirectionPin,LOW);
  digitalWrite(MotorLSpeedPin,LOW);
}

void moveForward(int x) {
  stepCntTarget=x;
  stepCntL=0;
  stepCntR=0;
  PIDi=0;
  moveForward();  
  while(stepCntL<stepCntTarget && (!useProxSensor || (digitalRead(ProxSensor1Pin) && digitalRead(ProxSensor2Pin)))) { 
    digitalWrite(EchoTriggerPin, HIGH);
    delayMicroseconds(10);
    digitalWrite(EchoTriggerPin, LOW); 
    dist=pulseIn(EchoPin,HIGH,60000)/58;
    if (dist>0&&dist<20) break;
  }
  stopMove();
}

void moveBackward(int x) {
  stepCntTarget=x;
  stepCntL=0;
  stepCntR=0; 
  PIDi=0;
  moveBackward();
  while(stepCntL<stepCntTarget);  
  stopMove();
}

void turnLeft(int x) {
  stepCntTarget=x;
  stepCntL=0;
  stepCntR=0;   
  turnLeft();
  while(stepCntL<stepCntTarget);  
  stopMove();  
}

void turnRight(int x) {
  stepCntTarget=x;
  stepCntL=0;
  stepCntR=0;   
  turnRight();
  while(stepCntL<stepCntTarget);  
  stopMove();
}

void pause(int x) {
  delay(x);
}

void setkc(int x) {
  Kc=double(x)/1000;
}

void setPwmInit(int x) {
  pwmInit=x;
}

void setspeed(int x) {
  speedTarget=x;
}

void leftLight(int x) {
    if (x==0) {
      LeftLightState=false;
      digitalWrite(LeftLightPin,LOW);
    }
    else if (x==2) {
      LeftLightState=false;
      digitalWrite(LeftLightPin,HIGH);
    }
    else {
        digitalWrite(LeftLightPin,HIGH);
        LeftLightState=true;
        if (!RightLightState) SideLightCnt=0;
    }
}

void rightLight(int x) {
    if (x==0) {
      RightLightState=false;
      digitalWrite(RightLightPin,LOW);
    }
    else if (x==2) {
      RightLightState=false;
      digitalWrite(RightLightPin,HIGH);
    }
    else {
      digitalWrite(RightLightPin,HIGH);
      RightLightState=true;
      if (!LeftLightState) SideLightCnt=0;
    }
    
}

void leftRearLight(int x) {
  if (x==0) digitalWrite(leftRearLightPin,LOW);
    else digitalWrite(leftRearLightPin,HIGH);
}

void rightRearLight(int x) {
  if (x==0) digitalWrite(rightRearLightPin,LOW);
    else digitalWrite(rightRearLightPin,HIGH);
}

void headLight(int x) {
  if (x==0) digitalWrite(HeadLightsPin,LOW);
    else digitalWrite(HeadLightsPin,HIGH);
}

void horn(int x) {
  if (x==0) digitalWrite(HornPin,LOW);
    else digitalWrite(HornPin,HIGH);
}

void help() {
  Serial.println("fw (n) : move forward (by n steps)");
  Serial.println("bw (n) : move backward (by n steps)");
  Serial.println("tl (n) : turn left (by n steps)");
  Serial.println("tr (n) : turn right (by n steps)");
  Serial.println("st : stop move");
  Serial.println("pa [n] : pause of n milliseconds");
  Serial.println("hl [x] : head light [0: OFF, 1: ON]");
  Serial.println("ho [x] : horn [0: OFF, 1: ON]");
  Serial.println("lr [x] : left rear light [0: OFF, 1: ON]");
  Serial.println("rr [x] : right read light [0: OFF, 1: ON]");
  Serial.println("ll [x] : left light [0: OFF, 1: CLIGN, 2:ON]");
  Serial.println("rl [x] : right light [0: OFF, 1: CLIGN, 2:ON]");
  Serial.println("ss [n] : set speed");
  Serial.println("sp : save program");
  Serial.println("ep : end of program");
  Serial.println("rp : run program");
  Serial.println("pp : print program");
  Serial.println("ma.... : forward command ... to MeArm");
  Serial.println("he : help");
}


void processCmd(String str) {
  Serial.print(str);
  Serial.print(" : ");

  int pos=str.indexOf(' ');
  boolean ok=1;
  
  if (str.startsWith("ma")) softSerial.println(str.substring(2)); // Forward command to MeArm
  else {
    if (pos==-1) {  // command with no arg
      if (str.compareTo("fw")==0 || str.compareTo("8")==0) moveForward();
      else if (str.compareTo("bw")==0 || str.compareTo("2")==0) moveBackward();
      else if (str.compareTo("tl")==0 || str.compareTo("4")==0) turnLeft();
      else if (str.compareTo("tr")==0 || str.compareTo("6")==0) turnRight();
      else if (str.compareTo("st")==0 || str.compareTo("5")==0) stopMove();
      else if (str.compareTo("sp")==0) {pgrmString="";inputMode=1;}
      else if (str.compareTo("rp")==0) execPgrm(pgrmString);
      else if (str.compareTo("pp")==0) Serial.println(pgrmString);
      else if (str.compareTo("he")==0) help();
      else ok=0;
    } else {  // command with at least 1 arg
      String cmd=str.substring(0,pos);
      int arg=str.substring(pos+1).toInt();
      if (cmd.compareTo("fw")==0) moveForward(arg);
      else if (cmd.compareTo("bw")==0) moveBackward(arg);
      else if (cmd.compareTo("tl")==0) turnLeft(arg);
      else if (cmd.compareTo("tr")==0) turnRight(arg);
      else if (cmd.compareTo("pa")==0) pause(arg);
      else if (cmd.compareTo("kc")==0) setkc(arg);    
      else if (cmd.compareTo("ss")==0) setspeed(arg);        
      else if (cmd.compareTo("ll")==0) leftLight(arg);        
      else if (cmd.compareTo("rl")==0) rightLight(arg);
      else if (cmd.compareTo("lr")==0) leftRearLight(arg);        
      else if (cmd.compareTo("rr")==0) rightRearLight(arg);
      else if (cmd.compareTo("hl")==0) headLight(arg);
      else if (cmd.compareTo("ho")==0) horn(arg);
      else ok=0;
    }
  }
  if (ok) Serial.println("OK");
    else Serial.println("Error");
  if (inputMode==1) Serial.println("Recording program");
}

void execPgrm(String str) {
  while(str.length()>0) {
    int endpos;
    str.trim();
    if (str.startsWith("repeat")) {
      int startpos=str.indexOf('[');
      int n=str.substring(6,startpos).toInt();
      int level=1;
      endpos=startpos;
      while(endpos<str.length()&&level>0) {
        endpos++;
        if (str.charAt(endpos)=='[') level++;
        if (str.charAt(endpos)==']') level--;
      }
      for (int i=0;i<n;i++) execPgrm(str.substring(startpos+1,endpos));
    } else {
      endpos=str.indexOf(';');
      if (endpos==-1) endpos=str.length();
      processCmd(str.substring(0,endpos));
    }
    if (endpos<str.length()-1) str=str.substring(endpos+1);
      else str="";
  }
}

void ISR0() {
  unsigned long intTime=micros();
  if ((intTime-prevIntTimeR)>IntDebounce && digitalRead(IntPin0)==HIGH) {
    intPeriodR=intTime-prevIntTimeR;
    prevIntTimeR=intTime;
    stepCntR++;
  }
}

void ISR1() {
  unsigned long intTime=micros();
  if ((intTime-prevIntTimeL)>IntDebounce && digitalRead(IntPin1)==HIGH) {
    intPeriodL=intTime-prevIntTimeL;
    prevIntTimeL=intTime;
    stepCntL++;    
  }
}

void timerISR() {
  if (PIDstate>0) {
    double target=speedTarget;
    if (PIDi<8) target=target*PIDi/8;
    double meas=3e6/double(intPeriodL);
    if (meas>1000) meas=0;
    double error=target-meas;
    pwmL=pwmL+Kc*error;
    if (pwmL>255) pwmL=255;
    if (pwmL<0) pwmL=0;
    lasterrorL=error;
    if (PIDstate==2) analogWrite(MotorLSpeedPin,255-pwmL);
      else analogWrite(MotorLSpeedPin,pwmL);
  
    meas=3e6/double(intPeriodR);
    if (meas>1000) meas=0;
    error=target-meas;
    pwmR=pwmR+Kc*error;
    if (pwmR>255) pwmR=255;
    if (pwmR<0) pwmR=0;
    lasterrorR=error;
    if (PIDstate==2) analogWrite(MotorRSpeedPin,255-pwmR);
      else analogWrite(MotorRSpeedPin,pwmR);

    PIDi++;    
  }

  SideLightCnt++;
  if (LeftLightState && (SideLightCnt%10)==0) digitalWrite(LeftLightPin,(SideLightCnt%20)==0);
  if (RightLightState && (SideLightCnt%10)==0) digitalWrite(RightLightPin,(SideLightCnt%20)==0);
}
MAX009 arduino code for MeArmArduino
Code to be uploaded in the arduino UNO mounted on the MeArm
#include <Servo.h>

Servo myservos[4];
const int servosPin[]={6,9,10,11};
const int initialPos[]={80,90,90,0};
const int button1Pin = 12;
const int button2Pin = 8;
const int ledPin = 2;
const int maxLineBuffer=80;

String lineBuffer;

int button1PreviousState=HIGH;
int button2PreviousState=HIGH;

// action state :
// 0 : stop
// 1 : turn left
// 2 : turn right
// 3 : go forward
// 4 : go backward
// 5 : go up
// 6 : go down
// 7 : open
// 8 : close
int state=0;  

void setup() {
  pinMode(button1Pin, INPUT);
  pinMode(button2Pin, INPUT);
  pinMode(ledPin, OUTPUT);
  digitalWrite(ledPin, HIGH);
  digitalWrite(button1Pin, HIGH);
  digitalWrite(button2Pin, HIGH);
  Serial.begin(9600);
  for (int i=0;i<4;i++) myservos[i].attach(servosPin[i]);  
  setInitialPos();
}

void loop() {
  // Listen to serial port
  while (Serial.available() > 0) {

    char c = Serial.read();
    if (c=='\n' || lineBuffer.length()==maxLineBuffer) {
        lineBuffer.trim();
        int pos=lineBuffer.indexOf(' ');
        if (pos==-1) {  // command with no arg
          if (lineBuffer.compareTo("tl")==0) state=1;       // Turn Left
          else if (lineBuffer.compareTo("tr")==0) state=2;  // Turn Right
          else if (lineBuffer.compareTo("fw")==0) state=3;  // move Forward
          else if (lineBuffer.compareTo("bw")==0) state=4;  // move Backward
          else if (lineBuffer.compareTo("up")==0) state=5;  // move Up
          else if (lineBuffer.compareTo("do")==0) state=6;  // move Down
          else if (lineBuffer.compareTo("op")==0) state=7;  // OPen claw
          else if (lineBuffer.compareTo("cl")==0) state=8;  // CLose claw
          else if (lineBuffer.compareTo("st")==0) state=0;  // STop
          else if (lineBuffer.compareTo("ip")==0) setInitialPos();  // reset to Initial Position
          else if (lineBuffer.compareTo("rp")==0) runProgram();     // Run Program
          else if (lineBuffer.compareTo("he")==0) printHelp();      // HElp
        } else {  // command with one arg
          String cmd=lineBuffer.substring(0,pos);
          int arg=lineBuffer.substring(pos+1).toInt();
          if (cmd.compareTo("s1")==0) {setPos(0,arg);printPos();}       // set Servo 1 position
          else if (cmd.compareTo("s2")==0) {setPos(1,arg);printPos();}  // set Servo 2 position
          else if (cmd.compareTo("s3")==0) {setPos(2,arg);printPos();}  // set Servo 3 position
          else if (cmd.compareTo("s4")==0) {setPos(3,arg);printPos();}  // set Servo 4 position
          else if (cmd.compareTo("li")==0) setLight(arg);               // set LIght [0: OFF, 1: ON]
        }
        lineBuffer="";
    }
    else lineBuffer+=c;
  }
  // Check joysticks
  if (analogRead(A0)<250) {myservos[1].write(myservos[1].read()-1);printPos();}
  if (analogRead(A0)>750) {myservos[1].write(myservos[1].read()+1);printPos();}
  if (analogRead(A1)<250) {myservos[0].write(myservos[0].read()+1);printPos();}
  if (analogRead(A1)>750) {myservos[0].write(myservos[0].read()-1);printPos();}
  if (analogRead(A2)<250) {myservos[2].write(myservos[2].read()-1);printPos();}
  if (analogRead(A2)>750) {myservos[2].write(myservos[2].read()+1);printPos();}
  if (analogRead(A3)<250) {myservos[3].write(myservos[3].read()-1);printPos();}
  if (analogRead(A3)>750) {myservos[3].write(myservos[3].read()+1);printPos();}
  // Check buttons
  if (digitalRead(button1Pin)==LOW && button1PreviousState==HIGH) {
    button1PreviousState=LOW;
    setInitialPos();
  }
  if (digitalRead(button1Pin)==HIGH) button1PreviousState=HIGH;
  if (digitalRead(button2Pin)==LOW && button2PreviousState==HIGH) {
    button2PreviousState=LOW;
    runProgram();
  }
  if (digitalRead(button2Pin)==HIGH) button2PreviousState=HIGH;
  // Check state
  switch(state) {
    case 1:
      myservos[0].write(myservos[0].read()+1);
      printPos();
      break;
    case 2:
      myservos[0].write(myservos[0].read()-1);
      printPos();
      break;      
    case 3:
      myservos[1].write(myservos[1].read()+1);
      printPos();
      break;
    case 4:
      myservos[1].write(myservos[1].read()-1);
      printPos();
      break;      
    case 5:
      myservos[2].write(myservos[2].read()+1);
      printPos();
      break;
    case 6:
      myservos[2].write(myservos[2].read()-1);
      printPos();
      break;      
    case 7:
      myservos[3].write(myservos[3].read()-1);
      printPos();
      break;
    case 8:
      myservos[3].write(myservos[3].read()+1);
      printPos();
      break;      
  }
  // Wait
  delay(25);
}

void setInitialPos() {
  for (int i=0;i<4;i++) setPos(i,initialPos[i]);
  printPos();
}

void setPos(int servo_idx,int targetPos) {
  int currentPos=myservos[servo_idx].read();
  if (targetPos>currentPos) {
    for (int i=currentPos;i<=targetPos;i++) {
      myservos[servo_idx].write(i);
      delay(25);
    }
  }
  if (targetPos<currentPos) {
    for (int i=currentPos;i>=targetPos;i--) {
      myservos[servo_idx].write(i);
      delay(25);
    }
  }
}

void setLight(int x) {
  digitalWrite(ledPin,!x);
}

void runProgram() {
  setPos(0,135);
  setPos(0,35);
  setPos(0,80);
  setPos(1,120);
  setPos(1,70);
  setPos(1,90);
  setPos(2,160);
  setPos(2,70);
  setPos(2,90);
  setPos(3,13);
  delay(500);
  setPos(3,31);
  delay(500);
  setPos(3,13);
  delay(500);
  setPos(3,0);
  printPos();
}

void printPos() {
  for (int i=0;i<4;i++) {
      Serial.print(myservos[i].read());
      Serial.print(" ");
    }
  Serial.println();  
}

void printHelp() {
  Serial.println("tl : turn left");
  Serial.println("tr : turn right"); 
  Serial.println("fw : move forward"); 
  Serial.println("bw : move backward"); 
  Serial.println("up : move up"); 
  Serial.println("do : move down"); 
  Serial.println("op : open claw"); 
  Serial.println("cl : close claw"); 
  Serial.println("st : stop move");
  Serial.println("ip : reset to initial position");
  Serial.println("rp : run program");
  Serial.println("he : print help"); 
  Serial.println("s1 [x] : move servo 1 to position x");
  Serial.println("s2 [x] : move servo 2 to position x");
  Serial.println("s3 [x] : move servo 3 to position x");
  Serial.println("s4 [x] : move servo 4 to position x");
  Serial.println("li [x] : set light [0: OFF, 1: ON]"); 
}

Schematics

MAX009 Schematic
MAX009 Schematic Fritzing source file
max009.fzz
MAX009 Schematic
Max009 Schematic PDF

Comments

Similar projects you might like

Hand Gesture Controlled Robot

Project tutorial by Mayoogh Girish

  • 58,755 views
  • 78 comments
  • 72 respects

Smartphone Controlled Arduino 4WD Robot Car

Project in progress by Andriy Baranov

  • 54,219 views
  • 43 comments
  • 103 respects

Make your first Arduino robot - The best beginners guide!

Project tutorial by Muhammed Azhar

  • 44,731 views
  • 15 comments
  • 101 respects

How to Make a Remote Controlled Robotic Hand with Arduino

Project showcase by Gabry295

  • 37,222 views
  • 5 comments
  • 117 respects

Android Controlled 6DoF Robotic Arm

Project in progress by Team Mans

  • 11,065 views
  • 0 comments
  • 17 respects
Add projectSign up / Login