Scribd
Upload
78 views

Dchome

The document describes the code for a linear stage with closed-loop control. It defines pin assignments for direction, voltage, brake and limit switch inputs. In setup, the pins are initialized and the stage is set to coast in the counterclockwise direction. The main loop moves the stage to the limit switch at 35 mm/s, then 10 mm away at 20 mm/s, back to the switch at 1 mm/s, and finally 30 mm away at 10 mm/s, each time monitoring speed with a PID control loop. Functions are used to count pulses from the encoder and calculate velocity.

Uploaded by

api-526001342
Copyright
© © All Rights Reserved
Available Formats
Download as PDF, TXT or read online on Scribd
78 views

Dchome

The document describes the code for a linear stage with closed-loop control. It defines pin assignments for direction, voltage, brake and limit switch inputs. In setup, the pins are initialized and the stage is set to coast in the counterclockwise direction. The main loop moves the stage to the limit switch at 35 mm/s, then 10 mm away at 20 mm/s, back to the switch at 1 mm/s, and finally 30 mm away at 10 mm/s, each time monitoring speed with a PID control loop. Functions are used to count pulses from the encoder and calculate velocity.

Uploaded by

api-526001342
Copyright
© © All Rights Reserved
Available Formats
Download as PDF, TXT or read online on Scribd
You are on page 1/ 2

//Linear Stage With Closed-Loop Control//

#define Dir 12 //HIGH = CW LOW = CCW(towards button)


#define Voltage 3 //High = 225, LOW = 0
#define Brake 9 //HIGH = On, LOW = Ogg
#define A 7 //Inputs
#define B 2
#define Limit 5 //Limit Switch

void setup() {
pinMode(12,OUTPUT); //Dir
pinMode(3,OUTPUT); //Voltage
pinMode(9,OUTPUT); //Brake
pinMode(7,INPUT); //Limmit switch
attachInterrorupt(digitalPinToInterrorupt(2),count,RISING);
pinMode(10,INPUT_PULLUP);
Serial.begin(9600);

digitalWrite(Voltage,LOW); //Motor OFF


digitalWrite(Brake,LOW); //Brake OFF
digitalWrite(Dir,LOW); //Direction CCW

while(digitalRead(Button)){}; //Press button to start, delay 1 second


delay(1000);
}

int v = 0, currmmps = 0;
float velcur = 0, error = 0;
volatile long int pulses = 0, ref_pulses = 0;
volatile int init_pulses = 0; //Define all varibales for movement functions

float K1 = 0.1 , K2 = 0.1 , K3 = 0.1; //Constraints set

void loop() {
//Move to switch at set speed of 35 mm/s until button is hit
digitalWrite(Dir,LOW);
v=50; //Set voltage
analogWrite(Voltage,v); //Set speed

while(digitalRead(Limit)){ //Waits for button press


velcur= speedset(50000);
error=(35-velcur);
v += K1*error;
if (v>255)
v=255;
else if (v<0)
v=0;
analogWrite(Voltage,v);
Serial.println(String(millis())+","+String(velcur)+","+String(pulses)+","+String(v)); //CSV file creation
}
analogWrite(Voltage,0);
delay(1000); //stop for 1 second
ref_pulses = pulses;

//Move 10 mm away from switch at 20 mm/s


digitalWrite(Dir,HIGH);
v=50;
analogWrite(Voltage,v);
while(pulses-ref_pulses< 10*374/8){ //10 mm
velcur=speedset(50000);
error=(20-velcur);
v += K2*error;
if (v>255)
v=255;
else if (v<0)
v=0;
analogWrite(Voltage,v);
Serial.println(String(millis())+"\t"+String(velcur)+"\t"+String(pulses)+"\t"+String(v)); //CSV file creation
}
stop1();
//Move back to switch at 1 mm/s
digitalWrite(Dir,LOW);
while(digitalRead(Limit)){
analogWrite(Voltage,45);
velcur=speedset(50000);
Serial.println(String(millis())+"\t"+String(velcur)+"\t"+String(pulses)+"\t"+String(v)); //CSV File creation
}
ref_pulses=pulses;
analogWrite(Voltage,0);
delay(1000);
Serial.println(String(millis())+"\t"+String(velcur)+"\t"+String(pulses)); //CSV file creation

//Move 30 mm away from the switch at 10 mm/s


digitalWrite(Dir,HIGH);
while(pulses-ref_pulses < 30*374/8){ //30 mm
v=50;
analogWrite(Voltage,v);
velcur=speedset(50000);
error=(10-velcur);
v += K3*error;
if (v>255)
v=255;
else if (v<0)
v=0;
analogWrite(Voltage,v);
Serial.println(String(millis())+"\t"+String(velcur)+"\t"+String(pulses)+"\t"+String(v)); //CSV file Creation
}
analogWrite(Voltage,0);
delay(1000);

//Turn off power to motor


shutdown();
while(true);
}

//Functions//
//Counting function, reads A terminal
void count(){
if(digitalRead(A)==HIGH)
pulses++;
else
pulses--;
}

//Velocity function for graphs


double speedset(long int us){
long int t0=0, initialPulses=0, delta_pulses=0, delta_t;
double mmps=0;
initialPulses=pulses;
t0=micros();
while(micros()-t0<us){}
delta_t=us;
delta_pulses=pulses-initialPulses;

mmps=double(delta_pulses)/double(delta_t)*1E+6 /374.0 * 8; //milimeters per second


return(mmps);
}

//Power off function


void shutdown() {
digitalWrite(Voltage,LOW);
digitalWrite(Brake,LOW);
}

You might also like