[專題] 第一組- 機械手臂

ppt

 

 #include
 #define echoPin 7
 #define trigPin 8

char key = 0;
const unsigned int BAUD_RATE = 9600;

Servo myservo1;
Servo myservo2;
Servo myservo3;
Servo myservo4;
Servo myservo5;


int pos1  = 90;  //initial position of servo 1
int pos2  = 90;
int pos3  = 90;
int pos4  = 90;



const int maxDeg  = 180; 
const int minDeg  = 0;    
const int maxDeg1 = 180;
const int minDeg1 = 0;
const int maxDeg2 = 180;
const int minDeg2 = 0;
const int maxDeg3 = 180;
const int minDeg3 = 0;


const int leftPin1  = 52;
const int rightPin1 = 50;
const int leftPin2  = 48;
const int rightPin2 = 46;
const int leftPin3  = 44;
const int rightPin3 = 42;
const int leftPin4  = 40;
const int rightPin4 = 38;



const int outputPin1 = 6; // pwm function will be disabled on pin 9 and 10 if using servo
const int outputPin2 = 3;
const int outputPin3 = 4;
const int outputPin4 = 5;
const int outputPin5 = 2;


int leftPressed1  = 0;
int rightPressed1 = 0;
int leftPressed2  = 0;
int rightPressed2 = 0;
int leftPressed3  = 0;
int rightPressed3 = 0;
int leftPressed4  = 0;
int rightPressed4 = 0;

void setup() 
{ 
myservo1.attach(outputPin1);  // attaches the servo on pin 9 to the servo object 
myservo2.attach(outputPin2);
myservo3.attach(outputPin3);
myservo4.attach(outputPin4);
myservo5.attach(outputPin5);
myservo5.write(outputPin5);
Serial.begin(BAUD_RATE);
pinMode(trigPin,OUTPUT);
pinMode(echoPin,INPUT);
} 

void loop() 
{ 
leftPressed1  = digitalRead(leftPin1);
rightPressed1 = digitalRead(rightPin1);
leftPressed2  = digitalRead(leftPin2);
rightPressed2 = digitalRead(rightPin2);
leftPressed3  = digitalRead(leftPin3);
rightPressed3 = digitalRead(rightPin3);
leftPressed4  = digitalRead(leftPin4);
rightPressed4 = digitalRead(rightPin4);
 
 if(leftPressed1){
  if(pos1 < maxDeg) 
   pos1 += 1;
   myservo1.write(pos1);              // tell servo to go to position in variable ‘pos’ 
  }

 
 if(rightPressed1){
  if(pos1 > minDeg) 
   pos1 -= 1;
   myservo1.write(pos1);              // tell servo to go to position in variable ‘pos’ 
  }
  
  
  if(leftPressed2){
  if(pos2 < maxDeg1)
   pos2 += 1;
   myservo2.write(pos2);              // tell servo to go to position in variable ‘pos’ 
  }

 
  if(rightPressed2){
  if(pos2 > minDeg1) 
   pos2 -= 1;
   myservo2.write(pos2);              // tell servo to go to position in variable ‘pos’ 
  }
  
  
  if(leftPressed3){
  if(pos3 < maxDeg2)
   pos3 += 1;
   myservo3.write(pos3);              // tell servo to go to position in variable ‘pos’ 
  }

 
  if(rightPressed3){
  if(pos3 > minDeg2) 
   pos3 -= 1;
   myservo3.write(pos3);              // tell servo to go to position in variable ‘pos’ 
  }
  
  
   if(leftPressed4){
  if(pos4 < maxDeg3)
   pos4 += 1;
   myservo4.write(pos4);              // tell servo to go to position in variable ‘pos’ 
  }

 
  if(rightPressed4){
  if(pos4 > minDeg3) 
   pos4 -= 1;
   myservo4.write(pos4);              // tell servo to go to position in variable ‘pos’ 
  }
  
  
   if(Serial.available()>0)
  {
      key = Serial.read();
      Serial.print("I received: ");
      Serial.println(key, DEC);

       if(key == byte(97))
     {
        Serial.print("Turning Motor 1 to 0 deg");
        myservo5.write(0);
     }
       else if(key==byte(115))
     {
        Serial.print("Turning Motor 1 to 180 deg");
        myservo5.write(180);
     }  
      delay(1000);
   }
      delay(15);                       // waits 15ms for the servo to reach the position 
      
      int distance, duration;
      digitalWrite(trigPin,HIGH); 
      delayMicroseconds(500);
      digitalWrite(trigPin,LOW);
      duration = pulseIn(echoPin,HIGH);
      distance = (duration/2)/29.1;
      Serial.print(distance);
      Serial.println("CM");
      
}