Components:
...
PID controls:
#include "Arduino.h"
#include "Wire.h"
#include "DFRobot_VL53L0X.h"
DFRobot_VL53L0X sensor;
const int pwmPin = 3;
const int dirPin = 12;
const int brakePin = 9;
const int xpin = A4;
const int ypin = A5;
const int y0 = 637;
const int switchpinNO = 8;
const int switchpinNC = 7;
const int distance0 = 305;
const int encoderpin = A1;
const int buttonpin = A2;
const int pourthreshold = 258;
int ref0;
int refencoder, drawpwm, error, error0;
float kp = .2;
float kd = 1.4;
const int drawff = 40;
...
states state, prior_state;
void setup() {
Serial.begin(115200);
digitalWrite(brakePin, LOW);
prior_state = NONE;
state = RESET;
Wire.begin();
//Set I2C sub-device address
sensor.begin(0x50);
//Set to Back-to-back mode and high precision mode
sensor.setMode(sensor.eContinuous, sensor.eHigh);
//Laser rangefinder begins to work
sensor.start();
//digitalWrite(dirPin,HIGH);
pinMode(buttonpin, INPUT);
}
void loop() {
distance = int(sensor.getDistance());
//int yvalue = analogRead(ypin);
//Serial.println(state);
//Serial.println(yvalue);
//Serial.println(switchstatus);
Serial.print("TOF Reading:"); Serial.println(distance);
Serial.println(analogRead(A1));
switch (state) {
case RESET:
Serial.println("RESET");
reset();
break;
case IDLE:
Serial.println("IDLE");
idle();
break;
case POUR:
Serial.println("POUR");
pour();
break;
case DRAW:
Serial.println("DRAW");
draw();
break;
case FINISH:
Serial.println("FINISH");
finish();
break;
}
//Serial.print("BUTTON: "); Serial.println(analogRead(A2));
//delay(50);
}
...
if (digitalRead(switchpinNO)) {
state = IDLE;
ref0 = analogRead(encoderpin);
}
if (prior_state != state) {
// leaving the reset state
drive(0, 0);
delay(500);
}
}
...
encoderval = analogRead(encoderpin);
...
} else {
drive(1, 0);
}
Serial.println("WAITING FOR USER INPUT"); // waits for user to press button
if (analogRead(buttonpin)> 1020){
state = POUR;
}
if (prior_state != state) {
// leaving idle state
}
}
void pour(){
int refencoder;
if (prior_state != state){
refencoder = 40;
}
distance = int(sensor.getDistance());
racounter ++;
encoderval = analogRead(encoderpin)-ref0;
Serial.print("ENCODER: ");
Serial.println(encoderval);
...
distance25 = distance + distance25;
if (racounter % 10 == 0){
distance25avg = distance25/10;
distance25 = 0;
}
Serial.print("distance: "); Serial.println(distance25avg);
if (distance25avg <= pourthreshold && distance25avg != 0){
state = DRAW;
Serial.println("Drawing");
// stop the flow
}
...
}
}
void draw(){
if (prior_state != state){
prior_state = state;
refencoder = 230;
}
encoderval = analogRead(encoderpin) - ref0;
error = refencoder - encoderval;
drawpwm = drawff + kp*abs(error) + kd * abs((error)-error0);
Serial.print("derror"); Serial.println((error)-error0);
if (drawpwm > 255) {
drawpwm = 255;
}
if (encoderval < refencoder){
drive(1,drawpwm);
}
else{
drive(1,0);
delay(2250);
state = FINISH;
}
...
void finish(){
if (prior_state != state){
prior_state = state;
}
encoderval = analogRead(encoderpin) - ref0;
distance = int(sensor.getDistance());
if (encoderval <= 70){
drive(0,0);
if (distance >= 290) {
state = RESET;
}
}
else{
drive(0,120);
}
}
void drive(int dir, int pwm) { // dir is 0 or 1
digitalWrite(dirPin, dir);
analogWrite(pwmPin, pwm);
}
We used PD control where kp = 0.2, kp = 1.4, and we feedback on the encoder reading to get errors, then we got the input PWM we need to control the motor speed.
Arduino code:
| View file | ||||
|---|---|---|---|---|
|