[PYTHON] Updated Rubik's Cube Robot software 6. Machine operation (Arduino)
What is this article?
I am currently developing a robot that solves a 2x2x2 Rubik's cube. This is a collection of commentary articles on the robot program.
I used to write an article collection represented by the article here, but since this time the software has been significantly updated so I will introduce a new program. think.
The corresponding code is available at here.
Related articles
"Let's make a robot that solves the Rubik's cube!"
Updated software for Rubik's Cube Robot
- Basic function
- Pre-calculation
- Solution Search
- Status recognition
- Machine operation (Python)
- Machine operation (Arduino) (this article)
- Main processing
This time, we will introduce `` `soltvvo3_arduino.ino``` as a machine operation (Arduino) edition.
Constants and variables
Globally placed constants and variables.
const int magnet_threshold = 50;
const long turn_steps = 400;
const int step_dir[2] = {11, 9};
const int step_pul[2] = {12, 10};
const int sensor[2] = {14, 15};
const int grab_deg[2] = {74, 74};
const int release_deg[2] = {96, 96};
const int offset = 3;
char buf[30];
int idx = 0;
long data[3];
The constant is the port number or the angle of the servo motor. Variables are used to split values given by separating them with spaces.
Around the servo library
Include the servo library in relation to using the servo motor and define `` `servo0, servo1```.
#include <Servo.h>
Servo servo0;
Servo servo1;
setup
Setup.
void setup() {
Serial.begin(115200);
for (int i = 0; i < 2; i++) {
pinMode(step_dir[i], OUTPUT);
pinMode(step_pul[i], OUTPUT);
pinMode(sensor[i], INPUT);
}
servo0.attach(7);
servo1.attach(8);
servo0.write(release_deg[0] + 5);
servo1.write(release_deg[1] + 5);
delay(70);
servo0.write(release_deg[0]);
servo1.write(release_deg[1]);
}
Define the input and output of the pin, and move the servo motor to move the arm outward.
Rotate the arm
A function that turns a stepping motor. I implemented trapezoidal drive, but for that reason it is a slightly uncool implementation that uses `` `delay``` without using a timer etc.
void move_motor(long num, long deg, long spd) {
bool hl = true;
if (deg < 0) hl = false;
digitalWrite(step_dir[num], hl);
long steps = abs(deg) * turn_steps / 360;
long avg_time = 1000000 * 60 / turn_steps / spd;
long max_time = 1500;
long slope = 50;
bool motor_hl = false;
long accel = min(steps / 2, max(0, (max_time - avg_time) / slope));
int num1 = (num + 1) % 2;
//acceleration
for (int i = 0; i < accel; i++) {
motor_hl = !motor_hl;
digitalWrite(step_pul[num], motor_hl);
delayMicroseconds(max_time - slope * i);
}
//Normal operation
for (int i = 0; i < steps * 2 - accel * 2; i++) {
motor_hl = !motor_hl;
digitalWrite(step_pul[num], motor_hl);
delayMicroseconds(avg_time);
}
//Decelerate
for (int i = 0; i < accel; i++) {
motor_hl = !motor_hl;
digitalWrite(step_pul[num], motor_hl);
delayMicroseconds(max_time - slope * accel + accel * (i + 1));
}
}
Basically, it is processed according to the following flow.
- Calculate the number of steps to turn from the angle
- Calculate the pulse width from the number of revolutions
- Calculate the number of steps used for acceleration and deceleration
Arm calibration
Calibrate the part of the arm that uses the stepping motor so that it has a proper angle. At this time, the magnet attached to the arm side and the hall sensor (magnetic sensor) attached to the stepping motor side are used.
void motor_adjust(long num, long spd) {
int max_step = 150;
int delay_time = 800;
bool motor_hl = false;
digitalWrite(step_dir[num], LOW);
while (analogRead(sensor[num]) <= magnet_threshold) {
motor_hl = !motor_hl;
digitalWrite(step_pul[num], motor_hl);
delayMicroseconds(delay_time);
}
while (analogRead(sensor[num]) > magnet_threshold) {
motor_hl = !motor_hl;
digitalWrite(step_pul[num], motor_hl);
delayMicroseconds(delay_time);
}
}
The first `` `while``` statement is tricky, but even if it is already determined that the arm is in the correct position, the angle of the arm may be slightly off, so the arm is shifted once. ..
The second while statement spins the stepper motor until the magnet is in front of the Hall sensor.
Release the puzzle
A function that moves the arm outward and releases the puzzle.
void release_arm(int num) {
if (num == 0)servo0.write(release_deg[num] + offset);
else servo1.write(release_deg[num] + offset);
}
It's not a very beautiful way of writing. If you know how to write beautifully, please let me know. For offset, I implemented an overshoot to move the motor faster, but I found that it was not necessary to return to the target value while overshooting, so I added offset It has become.
Grab the puzzle
It is a function that moves the arm around the center and grabs the puzzle.
void grab_arm(int num) {
if (num == 0)servo0.write(grab_deg[num] - offset);
else servo1.write(grab_deg[num] - offset);
delay(70);
if (num == 0)servo0.write(grab_deg[num]);
else servo1.write(grab_deg[num]);
}
The overshoot created with offset is useful here. I hope this will help you grab the puzzle faster (I don't know how effective it really is)
Main loop
The main loop receives the command and calls the corresponding function.
void loop() {
while (1) {
if (Serial.available()) {
buf[idx] = Serial.read();
if (buf[idx] == '\n') {
buf[idx] = '\0';
data[0] = atoi(strtok(buf, " "));
data[1] = atoi(strtok(NULL, " "));
data[2] = atoi(strtok(NULL, " "));
if (data[1] == 1000) grab_arm(data[0]);
else if (data[1] == 2000) release_arm(data[0]);
else if (data[1] == 0) motor_adjust(data[0], data[2]);
else move_motor(data[0], data[1], data[2]);
idx = 0;
}
else {
idx++;
}
}
}
}
The function to call depends on the type of command.
Summary
This time, I introduced the Arduino program that actually moves the actuator. I think it's partly not clean, probably because of my lack of knowledge of the language. Please tell us a good way to write in the comments.
Next time, I will explain the main processing of Python.
Recommended Posts