Mr.Moboto draws a 'stauche

moboto

#include <Servo.h> 

//Servo
Servo armServo;
Servo baseServo;
const int ARM_SERVO = 9;
const int BASE_SERVO = 10;

// motor constants & limitations
const int minArmServoPos = 30; //don't go lower, will hit the base
const int maxArmServoPos = 90;
const int minBaseServoPos = 60; //30; //0; // 0 is actual motor min
const int maxBaseServoPos = 120; //160; //180; //180 is actual motor max

int baseMidPoint = maxBaseServoPos - minBaseServoPos;
int startX;

// drawing variables
unsigned int basePosInc = 1;

int targetBasePos = maxBaseServoPos;

boolean isFinishedDrawing = false;

int moustache[88][2] = {
{0,0 },
{0,30},
{1,30},
{1,0 },
{2,0 },
{2,30},
{3,30},
{3,0 },
{4,0 },
{4,30},
{5,30},
{5,0 },
{6,0 },
{6,30},
{7,30},
{7,0 },
{8,0 },
{8,30},
{9,30},
{9,0 },
{10,0 },
{10,30},
{11,30},
{11,2 },
{11,4 },
{12,4 },
{12,30},
{13,30},
{13,6 },
{13,8 },
{14,8 },
{14,30},
{15,30},
{15,10},
{15,12},
{16,12},
{16,30},
{17,30},
{17,14},
{17,16},
{18,16},
{18,30},
{19,30},
{19,16},
{20,16},
{20,30},
{21,30},
{21,14},
{22,14},
{22,12},
{23,30},
{23,30},
{23,10},
{24,10},
{24,8 },
{24,30},
{25,30},
{25,6 },
{26,6 },
{26,4 },
{26,30},
{27,30},
{27,2 },
{28,2 },
{28,0 },
{29,0 },
{29,30},
{30,30},
{30,0 },
{31,0 },
{31,30},
{32,30},
{32,0 },
{33,0 },
{33,30},
{34,30},
{34,0 },
{35,0 },
{35,30},
{36,30},
{36,0 },
{37,0 },
{37,30},
{38,30},
{38,0 },
{39,0 },
{39,30} };

void setup(){
  
  //Servo setup
  armServo.attach(ARM_SERVO);  //attaches the servo on pin 9 to the servo object 
  baseServo.attach(BASE_SERVO);  //attaches the servo on pin 9 to the servo object 
  
  //Go to the first spot
  isFinishedDrawing = false;
  
  //baseMidPoint
  int minX = 0;
  int maxX = 39;
  startX = 90 + 4 - maxX/2;
  
  int startY = minArmServoPos + moustache[0][1];
  baseServo.write(startX); 
  armServo.write(startY); 
  
  delay(1000); //delay so the servo has time to get to first position
    
  //Debug setup
  Serial.begin(115200);
  
}

void loop(){
  
//don't keep on repeating the drawing
if ( !isFinishedDrawing ) {
   
 
  int length = sizeof(moustache)/sizeof(int)/2;
   
  //go through each item in the array
  for ( int i=0; i<length; i++) {
    
   int x = moustache[i][0];
   int y = moustache[i][1];
   
   Serial.print("     x: ");
   Serial.print(x);
   Serial.print(",     y: ");
   Serial.println(y);
   
   int baseServoPos = startX + x; //calculate offset
   //int armServoPos = maxArmServoPos - y; //calculate offset
   int armServoPos = minArmServoPos + y; //calculate offset
      
   Serial.print("base x: ");
   Serial.print(baseServoPos);
   Serial.print(", arm y: ");
   Serial.println(armServoPos);
   
   //move base first
   baseServo.write(baseServoPos);
   delay(200);
   
   //then move arm
   armServo.write(armServoPos);
   delay(200);
    
  }
  
  //finished loop so set boolean
  isFinishedDrawing = true;
  Serial.println(isFinishedDrawing);
  Serial.println("//////////");
  
}else{
 delay(60000);
}
  
}