Scribd
Upload
50 views5 pages

Designproject 2 Code

This code controls a motor to perform a sequence of approaches and retreats at different speeds. It uses interrupts to count motor pulses and calculate RPM. It takes readings over time to control the motor speed using PID. It performs: 1) an approach until a switch is pressed, 2) a retreat of 10mm, 3) an approach until the switch, 4) a retreat of 30mm, then stops. It prints the time, pulses, motor voltage, and RPM during each movement.

Uploaded by

api-533012799
Copyright
© © All Rights Reserved
Available Formats
Download as DOCX, PDF, TXT or read online on Scribd
Download as docx, pdf, or txt
50 views5 pages

Designproject 2 Code

This code controls a motor to perform a sequence of approaches and retreats at different speeds. It uses interrupts to count motor pulses and calculate RPM. It takes readings over time to control the motor speed using PID. It performs: 1) an approach until a switch is pressed, 2) a retreat of 10mm, 3) an approach until the switch, 4) a retreat of 30mm, then stops. It prints the time, pulses, motor voltage, and RPM during each movement.

Uploaded by

api-533012799
Copyright
© © All Rights Reserved
Available Formats
Download as DOCX, PDF, TXT or read online on Scribd
Download as docx, pdf, or txt
Download as docx, pdf, or txt
You are on page 1/ 5

void setup() {

pinMode(7, INPUT);
attachInterrupt(digitalPinToInterrupt(2), count, RISING);
Serial.begin(115200);

pinMode(12, OUTPUT); // DIR


pinMode(3, OUTPUT); // VOLTAGE
pinMode(9, OUTPUT); // BRAKE
pinMode(10, INPUT_PULLUP); // Switch
digitalWrite(9, LOW); // DISABLE BREAK

while (digitalRead(10)) {};


delay(1000);
}

volatile long int pulses = 0;


long int initial_pulses = 0, dPulses = 0;
double v_motor, current_rpm = 0, error;
int target;
unsigned long int t0 = 0, initialPulses = 0, delta_pulses = 0, delta_t, t;

void loop() {
//beginning of sequence
// 1ST APPROACH at 35mm/s (262.5rpm), stops when switch is pressed; HIGH is CW and
Positive pulses

t = micros();
v_motor = 175; // Change depending on load to reach target
Serial.println(" 1st Approach");
while (digitalRead(10) == HIGH) {
target = 262;
digitalWrite(12, HIGH);
analogWrite(3, v_motor);
current_rpm = getSpeed(100000);
error = (target - current_rpm);
v_motor = v_motor + 0.1 * error;
//print necessary data for the displacement and velocity graphs
Serial.print("Time:");
Serial.print("\t");
Serial.print(micros());
Serial.print("\t\t");
Serial.print("Pulses:");
Serial.print("\t");
Serial.print(pulses);
Serial.print("\t\t");
Serial.print("v_motor:");
Serial.print("\t");
Serial.print(v_motor);
Serial.print("\t\t");
Serial.print("RPM:");
Serial.print("\t");
Serial.println(current_rpm);

delayMicroseconds(3000);

// 1ST RETREAT 10mm (467.5 pulses) at 20 mm/s(150 rpm); LOW is CCW


v_motor = 40; // Change depnding on load to reach target
Serial.println(" 1st Retreat");
initial_pulses = pulses;
while (dPulses <= 467) {
target = 150;
dPulses = initial_pulses - pulses;
digitalWrite(12, LOW);
analogWrite(3, v_motor);
current_rpm = getSpeed(100000);
error = target - current_rpm;
v_motor = v_motor + 0.1 * error;

Serial.print("Time:");
Serial.print("\t");
Serial.print(micros());
Serial.print("\t\t");
Serial.print("Pulses:");
Serial.print("\t");
Serial.print(pulses);
Serial.print("\t\t");
Serial.print("v_motor:");
Serial.print("\t");
Serial.print(v_motor);
Serial.print("\t\t");
Serial.print("RPM:");
Serial.print("\t");
Serial.println(current_rpm);

}
// Stops between approaches, gives motor time to complete rotations
digitalWrite(3,0);
delay(700);

// 2ND APPROACH at lowest speed mm/s, stops when switch is pressed


v_motor = 40; // Change depnding on load to reach target
Serial.println(" 2nd Approach");

while (digitalRead(10) == HIGH) {


digitalWrite(12, HIGH);
analogWrite(3, v_motor);
current_rpm = getSpeed(100000);

Serial.print("Time:");
Serial.print("\t");
Serial.print(micros());
Serial.print("\t\t");
Serial.print("Pulses:");
Serial.print("\t");
Serial.print(pulses);
Serial.print("\t\t");
Serial.print("v_motor:");
Serial.print("\t");
Serial.print(v_motor);
Serial.print("\t\t");
Serial.print("RPM:");
Serial.print("\t");
Serial.println(current_rpm);

delayMicroseconds(3000); //gives motor time to complete rotations

// 2ND RETREAT 30 mm(1402.5 pulses) at 10 mm/s(75 rpm)


dPulses = 0;
Serial.println(" 2nd Retreat");
initial_pulses = (pulses);
v_motor = 55;
while (abs(dPulses) <= 1402) {
dPulses = (initial_pulses) - (pulses);
target = 75;
digitalWrite(12, LOW);
analogWrite(3, v_motor);
current_rpm = getSpeed(100000);
error = target - current_rpm;
v_motor = v_motor + 0.1 * error;

Serial.print("Time:");
Serial.print("\t");
Serial.print(micros());
Serial.print("\t\t");
Serial.print("Pulses:");
Serial.print("\t");
Serial.print(pulses);
Serial.print("\t\t");
Serial.print("v_motor:");
Serial.print("\t");
Serial.print(v_motor);
Serial.print("\t\t");
Serial.print("RPM:");
Serial.print("\t");
Serial.println(current_rpm);

// end of sequence, stops


digitalWrite(3, 0);
delay(2000);
while (true);
}

void count() {
if (digitalRead(7) == LOW)
pulses++;
else
pulses--;
}

double getSpeed(long int us) {


long int t0 = 0, initialPulses = 0, delta_pulses = 0, delta_t;
double rpm = 0;
initialPulses = pulses; // Set initial pulse count
t0 = micros(); // Set initial time, in us
while (micros() - t0 < us) {} // Allow sampling time (microseconds) to elapse
delta_t = us; // Duration of the sample, in microseconds
delta_pulses = abs(pulses - initialPulses); // Pulse increase during sample period
rpm = double(delta_pulses) / double(delta_t) * 1E+6 / 374.0 * 60.0; // w=d(theta)/dt,
conversion to r.p.m.
// Encoder gives 374 pulses/rev
return (rpm);
}

You might also like