Closed-Loop Control System

By Dan Chen, April 7, 2015

For my closed loop control system, I would like to monitor the pressure on the grabber to vary the grabbing speed of my grabber.

I ordered 2 FSRs (Force Sensitive Resistor), their actuation Force as low as 2 grams.

Here is how I wired them

09673-02

Making the 2-Finger Parallel Grippers

i used two servos to drive the fingers, however this is not a good design because some of the torque is wasted moving along the track. My next design would use either gears or linear actuators.

The Code

The code reads the pressure sensors data for every cycle that the servo is moving one position to another (closing). The grabber then pickup the item and drops it.

If the pressure passes certain threshold, the grabber would move slightly tighter to get a better grip, then lift the item up and drops it. I call it playing fetch with the robot.

int pos;
int reopen_count;



#include  
// servo
Servo servoBase1;  // create servo object to control a servo 
Servo servoBase2;  // create servo object to control a servo 
Servo servoRotateBase1;  // create servo object to control a servo 
Servo servoRotateBase2;  // create servo object to control a servo 


// pressure sensor
int senpin1=A1;
int senpin2=A2;
int sensorValue1;
int sensorValue2;

int avgpressure;

boolean keep_running=true;
  
// the setup routine runs once when you press reset:
void setup() {
  Serial.begin(9600);
  servoBase1.attach(A3);
  servoBase2.attach(A4);
  servoRotateBase1.attach(A5);
  servoRotateBase2.attach(A6);
  armdown();
}

// the loop routine runs over and over again forever:
void loop() {



  if (keep_running){
             //first number pos=? is how far apart
             //pos <= is how tight is the gap
             for(pos = 60; pos <= 113; pos += 1) // goes from 0 degrees to 180 degrees              {                    // in steps of 1 degree                servoBase1.write(180-pos);                servoBase2.write(pos);               //slow down press in slowly                //delay(avgpressure+25);                       // waits 15ms for the servo to reach the position                delay(50);                      // waits 15ms for the servo to reach the position                              read_pressure();                   if (sensorValue1>35  ||  sensorValue2>35){
                      keep_running=false;
                      servoBase1.write(180-pos+15); 
                       servoBase2.write(pos+15);
                       delay(100);                      // waits 15ms for the servo to reach the position 
                       pos=10000;
                      armup();
                  }
            }
  }
 /*
 if (keep_running){   
            for(pos = 113; pos>=60; pos-=1)     // goes from 180 degrees to 0 degrees 
            {            
              read_pressure();    
              servoBase1.write(180-pos); 
              servoBase2.write(pos);
              //delay(avgpressure+25);                       // waits 15ms for the servo to reach the position 
              delay(155);                       // waits 15ms for the servo to reach the position 
              
              if (sensorValue2>30){
                      keep_running=false;
                      pos=0;
                  }
            } 
  }
  */
  else{
    reopen_count++;
    if (reopen_count>200){
      reopen();
      keep_running=true;
      reopen_count=0;
    }
    
    delay(20);
  }



      

}

void reopen(){
            pos = 60;            
              servoBase1.write(180-pos); 
              servoBase2.write(pos);
              //delay(avgpressure+25);                       // waits 15ms for the servo to reach the position 
              armdown();
              delay(1000);                       // waits 15ms for the servo to reach the position 
            
            
}
void armup(){
//first number is how far apart
         for(pos = 90; pos <= 125; pos += 1) // goes from 0 degrees to 180 degrees          {                                  // in steps of 1 degree            servoRotateBase1.write(180-pos);            servoRotateBase2.write(pos);           delay(60);                       // waits 15ms for the servo to reach the position          }          } void armdown(){ //first number is how far apart                 for(pos = 112; pos>=75; pos-=1)     // goes from 180 degrees to 0 degrees 
        {                                
          servoRotateBase1.write(180-pos); 
          servoRotateBase2.write(pos);
          delay(60);                       // waits 15ms for the servo to reach the position 
        } 
}

void  read_pressure(){
//  
//
  // read the input on analog pin 0:
  // pressure different offset and zero it
  sensorValue1 = analogRead(senpin1);
  sensorValue2 = analogRead(senpin2);
  
  avgpressure=(sensorValue1+sensorValue2)/2;
  
  // print out the value you read:
  Serial.print(sensorValue1);
  Serial.print("t");
  Serial.println(sensorValue2);
  //delay(250);        // delay in between reads for stability
}

Download Code

Modular Robot Body Update

The robot can now mounted on a base, which we can mounted onto a table or a moving platform.

IMG_5255 IMG_5263

The robot now has 90 degree PVC pipe design, I could probably just purchase this but it didn’t talk all that long to make. Simile to PVC catalogs, the robot should have adopters, splitters, caps, different end effects .

DSC01924

 

Playing Fetch with the Robot