Drawing Robot

gistfile1.txt

#include <Servo.h> 

//Servo
Servo armServo;
const int ARM_SERVO = 9;

Servo baseServo;
const int BASE_SERVO = 10;

// drawing variables
unsigned int armPosInc = -10;
const int minArmServoPos = 30; //don't go lower, will hit the base
const int maxArmServoPos = 90;
const int startArmServoPos = minArmServoPos;

int armServoPos = startArmServoPos;
int targetArmPos = minArmServoPos;

boolean armTargetReached = false;

// base variables
unsigned int basePosInc = 1;
const int minBaseServoPos = 0; //don't go lower, will hit the base
const int maxBaseServoPos = 180;
const int startBaseServoPos = minBaseServoPos;

int baseServoPos = startBaseServoPos;
int targetBasePos = maxBaseServoPos;

boolean baseTargetReached = false;

void setup(){
  
  //Servo setup
  armServo.attach(ARM_SERVO);  //attaches the servo on pin 9 to the servo object 
  armServo.write(startArmServoPos); //reset

  baseServo.attach(BASE_SERVO);  //attaches the servo on pin 9 to the servo object 
  baseServo.write(startBaseServoPos); //reset  
  
  delay(1000); //delay so the servo has time to reset before the dc motoer kicks in
    
  //Debug setup
  Serial.begin(115200);
}

void loop(){
  
  //TODO: 
  //targetArmPos needs to come from somewhere...
  
  /*
  // ******** ARM ******
  if ( armServoPos != targetArmPos ) {
    // HASN'T REACHED POSITION SO KEEP MOVING
    
    armServoPos += armPosInc; //set new temp target position
    armServo.write(armServoPos); //update servo position
    
  }else{
    //REACHED TARGET!
    
    setTargetArmPos();

  }
  */
  
  
  // ******** BASE ******
  //Serial.println( baseServoPos );
  if ( baseServoPos != targetBasePos ) {
    // HASN'T REACHED POSITION SO KEEP MOVING
    //todo: need better condition if not incrementing by 1
    baseServoPos += basePosInc; //set new temp target position
 
    baseServo.write(baseServoPos); //update servo position
  }else{
    Serial.println( "target reached" );
    baseTargetReached = true;
  }
  
  
  //if ( armTargetReached && baseTargetReached ) {
  //if ( armTargetReached ) {  
  if ( baseTargetReached ) {
    
    
    delay(1000);
    
    //armServo.write(armServoPos);
    //setTargetArmPos();
    
    Serial.println("arm stuff");
    if ( armServoPos == minArmServoPos ) {
      for ( int i=minArmServoPos; i<=maxArmServoPos; i++) {
        armServoPos++; //set new temp target position
        armServo.write(armServoPos); //update servo position
        delay(100);
      }
    }else{
      for ( int i=maxArmServoPos; i>=minArmServoPos; i--) {
        armServoPos--; //set new temp target position
        armServo.write(armServoPos); //update servo position
        delay(100);
      }
    }
    
    setTargetBasePos();
    
    //armTargetReached = false;
    baseTargetReached = false;
    
  }else{
    delay(50);
  }
  
 
  
}

void setNewBaseTarget() {

}

void setTargetBasePos() {
 if (baseServoPos == minBaseServoPos ) {
   targetBasePos = maxBaseServoPos;
 }else{
   targetBasePos = minBaseServoPos;
 }
 int delta = targetBasePos - baseServoPos;
 if ( delta < 0 ) {
   basePosInc = -1;
 }else{
   basePosInc = 1;
 }
 Serial.println( "new MIN-MAX BASE target selected" );
 Serial.println( targetBasePos );
 Serial.println( basePosInc );
}

void setTargetArmPos() {
 if (armServoPos == minArmServoPos ) {
   targetArmPos = maxArmServoPos;
 }else{
   targetArmPos = minArmServoPos;
 }
 int delta = targetArmPos - armServoPos;
 if ( delta < 0 ) {
   armPosInc = -1;
 }else{
   armPosInc = 1;
 }
 Serial.println( "new MIN-MAX ARM target selected" );
 Serial.println( targetArmPos );
 Serial.println( armPosInc );
}