Skip to content

Instantly share code, notes, and snippets.

@fxprime

fxprime/visual_servoing_control_unit.ino

Last active April 13, 2021 14:11
Show Gist options
  • Save fxprime/0b06f3f8568f172beb3742e7bf333938 to your computer and use it in GitHub Desktop.
Save fxprime/0b06f3f8568f172beb3742e7bf333938 to your computer and use it in GitHub Desktop.
Download ZIP
Raw
visual_servoing_control_unit.ino
/*************************************************************
* *
* Visual servoing control unit *
* (Manual mode) *
* *
************************************************************/
/* -------------------------------------------------------------------------- */
/* Library */
/* -------------------------------------------------------------------------- */
/* ------------------------ Arduino Utility functions ----------------------- */
#include <Arduino.h>
/* ------------------------ Overall program settings ------------------------ */
// #define SERIALMON // Uncomment this line to debug analog input by serial monitor
#define STEPPER_FACTOR 60 // Speed of stepper motor for manual control define here
#define SERVO_STEP 1 // Amount of angle for servo motor per iteration (1/10 s per iteration)
/* ----------------------------- Stepping motor ----------------------------- */
#include <AccelStepper.h>
#define X_DIRPIN 5 // Define stepper motor control signal
#define X_STEPPIN 2 // each axis per stepper motor contain
#define Y_DIRPIN 6 // 3 pins and 1 enable pin which is a power
#define Y_STEPPIN 3 // gate for all stepper motor
#define Z_DIRPIN 7
#define Z_STEPPIN 4
#define ENABLEPIN 8
#define MOTORINTERFACETYPE 1
// Create a new instance of the AccelStepper class, one class per one stepping motor
AccelStepper x_stepper = AccelStepper(MOTORINTERFACETYPE, X_STEPPIN, X_DIRPIN);
AccelStepper y_stepper = AccelStepper(MOTORINTERFACETYPE, Y_STEPPIN, Y_DIRPIN);
AccelStepper z_stepper = AccelStepper(MOTORINTERFACETYPE, Z_STEPPIN, Z_DIRPIN);
/* ------------------------------- Servo motor ------------------------------ */
#include <Servo.h>
#define SERVOPIN 11 // Define the servo signal pin
Servo servo; // Define The name of servo motor
int servo_pose = 90; // Define servo variables ,set the start position of servo
/* -------------------------------------------------------------------------- */
/* Joystick */
/* -------------------------------------------------------------------------- */
/* ----------------------------- Joystick pinout ---------------------------- */
#define A_VRX A1 // Pin for X-axis of joystick A
#define A_VRY A2 // Pin for Y-axis of joystick A
#define A_SW 9 // Pin for switch of joystick A
#define B_VRX A0 // Pin for X-axis of joystick B
#define B_VRY A3 // Pin for Y-axis of joystick B
#define B_SW 10 // Pin for switch of joystick B
/* -------------------------------------------------------------------------- */
/* Utility variable */
/* -------------------------------------------------------------------------- */
const unsigned long period = 100; // Time period for joystick function in millisecond
unsigned long lastExecute; // This variable is used to stamp the time of joystick function.
/* -------------------------------------------------------------------------- */
/* Utility function prototype */
/* -------------------------------------------------------------------------- */
static inline void joystick(); // Get joystick inputs and update desired stepper motor
// speed and desired position of servo
/* -------------------------------------------------------------------------- */
/* Startup function */
/* -------------------------------------------------------------------------- */
void setup()
{
/* -------------------------- Serial monitor setup -------------------------- */
#ifdef SERIALMON
Serial.begin(115200);
#endif
/* -------------------------- Stepping motors setup ------------------------- */
pinMode(ENABLEPIN, OUTPUT); // Set pinout of enable pin
digitalWrite(ENABLEPIN, LOW); // and set state to LOW in order to enable motors
x_stepper.setMaxSpeed(8000); // Set maximum stepping motor speed 8000 step/s
y_stepper.setMaxSpeed(8000);
z_stepper.setMaxSpeed(8000);
/* ---------------------------- Servo motor setup --------------------------- */
servo.attach(SERVOPIN); // Set the servo pin (11)
servo.write(servo_pose);
/* --------------------- Set current time as start time --------------------- */
lastExecute = millis();
}
/* -------------------------------------------------------------------------- */
/* Runtime function */
/* -------------------------------------------------------------------------- */
void loop()
{
/* -------------- Limit the frequency to call joystick function ------------- */
unsigned long currentTime = millis(); // get the current "time" (actually the number of milliseconds since the program started)
if (currentTime - lastExecute >= period) // Check whether the period has elapsed
{
joystick(); // Get joystick inputs and update desired stepper motor
// speed and desired position of servo
lastExecute = currentTime; // Save current time, wait for next period.
}
/* ------ Execute stepping motor to run at desired speed that set from ------ */
x_stepper.runSpeed();
y_stepper.runSpeed();
z_stepper.runSpeed();
/* ----------------- Execute servo motor to position desired ---------------- */
servo.write(servo_pose);
}
/* -------------------------------------------------------------------------- */
/* Joystick to Stepper speed and servo position function */
/* -------------------------------------------------------------------------- */
static inline void joystick()
{
int A_vrx_data = 0; // Data of X-axis from joystick A
int A_vry_data = 0; // Data of Y-axis from joystick A
int B_vrx_data = 0; // Data of X-axis from joystick B
int B_vry_data = 0; // Data of Y-axis from joystick B
int servo_speed = 0; // Servo speed input
/* --------------------- read value of joystick A and B --------------------- */
A_vrx_data = analogRead(A_VRX);
A_vry_data = analogRead(A_VRY);
B_vrx_data = analogRead(B_VRX);
B_vry_data = analogRead(B_VRY);
/* --------- Assign servo speed to joystick B and convert to [-3,3]. -------- */
servo_speed = B_vry_data;
servo_speed = map(servo_speed, 0, 1023, -3000, 3000)/1000.0;
#ifdef SERIALMON
Serial.print("AVrx:");
Serial.print(A_vrx_data);
Serial.print("++");
Serial.print(A_vry_data);
Serial.print("--------");
Serial.print("BVrx:");
Serial.print(B_vrx_data);
Serial.print("++");
Serial.println(B_vry_data);
#endif
/* ----------- Convert X-axis on joystick A to x-stepper motor speed ---------- */
if ((A_vrx_data > 450) && (A_vrx_data < 550)) x_stepper.setSpeed(0);
else if (A_vrx_data > 550) x_stepper.setSpeed((A_vrx_data - 550)*STEPPER_FACTOR);
else if (A_vrx_data < 450) x_stepper.setSpeed((A_vrx_data - 450)*STEPPER_FACTOR);
/* ----------- Convert Y-axis on joystick A to y-stepper motor speed ---------- */
if ((A_vry_data > 450) && (A_vry_data < 550)) y_stepper.setSpeed(0);
else if (A_vry_data > 550) y_stepper.setSpeed((A_vry_data - 550)*STEPPER_FACTOR);
else if (A_vry_data < 450) y_stepper.setSpeed((A_vry_data - 450)*STEPPER_FACTOR);
/* ----------- Convert X-axis on joystick B to z-stepper motor speed ---------- */
if ((B_vrx_data > 450) && (B_vrx_data < 550)) z_stepper.setSpeed(0);
else if (B_vrx_data > 550) z_stepper.setSpeed((B_vrx_data - 550)*STEPPER_FACTOR);
else if (B_vrx_data < 450) z_stepper.setSpeed((B_vrx_data - 450)*STEPPER_FACTOR);
/* ----------- Convert Y-axis on joystick B to S-servo motor position --------- */
if ((B_vry_data > 450) && (B_vry_data < 550)) ;// Do nothing
else if (B_vry_data > 550)
{
servo_pose = servo_pose + (SERVO_STEP * servo_speed);
if (servo_pose > 180) servo_pose = 180;
}
else if (B_vry_data < 450)
{
servo_pose = servo_pose + (SERVO_STEP * servo_speed);
if (servo_pose < 0) servo_pose = 0;
}
}
/* -------------------------------------------------------------------------- */
/* End of joystick to Stepper speed and servo position function */
/* -------------------------------------------------------------------------- */
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment