Marco Newman / CartesianRobot

Important changes to repositories hosted on mbed.com

Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.

To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.

It is also possible to export all your personal repositories from the account settings page.

Dependents:   CartesianRobot_Demo

CartesianRobot.cpp@1:0b705e9f84ce, 2021-12-16 (annotated)

Committer:
marcoanewman
Date:
Thu Dec 16 04:33:27 2021 +0000
Revision:
1:0b705e9f84ce
Parent:
0:5af6295f4ec9 
consistency fixes

Who changed what in which revision?

UserRevisionLine numberNew contents of line
marcoanewman 0:5af6295f4ec9 1 #include "CartesianRobot.h"
marcoanewman 0:5af6295f4ec9 2
marcoanewman 0:5af6295f4ec9 3 ////////////////////////////////////////////////////////////////////////////////
marcoanewman 0:5af6295f4ec9 4 // Constructor & Destructor //
marcoanewman 0:5af6295f4ec9 5 ////////////////////////////////////////////////////////////////////////////////
marcoanewman 0:5af6295f4ec9 6 CartesianRobot::CartesianRobot(
marcoanewman 0:5af6295f4ec9 7 PinName x_step, PinName x_dir, PinName x_en,
marcoanewman 0:5af6295f4ec9 8 PinName y_step, PinName y_dir, PinName y_en,
marcoanewman 0:5af6295f4ec9 9 PinName z_step, PinName z_dir, PinName z_en
marcoanewman 0:5af6295f4ec9 10 ) : _x_stepper(x_step, x_dir, x_en),
marcoanewman 0:5af6295f4ec9 11 _y_stepper(y_step, y_dir, y_en),
marcoanewman 0:5af6295f4ec9 12 _z_stepper(z_step, z_dir, z_en)
marcoanewman 0:5af6295f4ec9 13 {
marcoanewman 0:5af6295f4ec9 14 _buffer_size = 0;
marcoanewman 0:5af6295f4ec9 15 }
marcoanewman 0:5af6295f4ec9 16
marcoanewman 0:5af6295f4ec9 17 CartesianRobot::~CartesianRobot(){}
marcoanewman 0:5af6295f4ec9 18
marcoanewman 0:5af6295f4ec9 19
marcoanewman 0:5af6295f4ec9 20 ////////////////////////////////////////////////////////////////////////////////
marcoanewman 0:5af6295f4ec9 21 // Setters & Getters //
marcoanewman 0:5af6295f4ec9 22 ////////////////////////////////////////////////////////////////////////////////
marcoanewman 0:5af6295f4ec9 23 void CartesianRobot::setXSpeed(float speed){_x_stepper.setSpeed(speed);}
marcoanewman 0:5af6295f4ec9 24 void CartesianRobot::setYSpeed(float speed){_y_stepper.setSpeed(speed);}
marcoanewman 0:5af6295f4ec9 25 void CartesianRobot::setZSpeed(float speed){_z_stepper.setSpeed(speed);}
marcoanewman 0:5af6295f4ec9 26
marcoanewman 0:5af6295f4ec9 27 void CartesianRobot::setXAcc(float acc){_x_stepper.setAcceleration(acc);}
marcoanewman 0:5af6295f4ec9 28 void CartesianRobot::setYAcc(float acc){_y_stepper.setAcceleration(acc);}
marcoanewman 0:5af6295f4ec9 29 void CartesianRobot::setZAcc(float acc){_z_stepper.setAcceleration(acc);}
marcoanewman 0:5af6295f4ec9 30
marcoanewman 0:5af6295f4ec9 31 void CartesianRobot::setXDec(float dec){_x_stepper.setDeceleration(dec);}
marcoanewman 0:5af6295f4ec9 32 void CartesianRobot::setYDec(float dec){_y_stepper.setDeceleration(dec);}
marcoanewman 0:5af6295f4ec9 33 void CartesianRobot::setZDec(float dec){_z_stepper.setDeceleration(dec);}
marcoanewman 0:5af6295f4ec9 34
marcoanewman 0:5af6295f4ec9 35 void CartesianRobot::enable(void)
marcoanewman 0:5af6295f4ec9 36 {
marcoanewman 0:5af6295f4ec9 37 _x_stepper.enable();
marcoanewman 0:5af6295f4ec9 38 _y_stepper.enable();
marcoanewman 0:5af6295f4ec9 39 _z_stepper.enable();
marcoanewman 0:5af6295f4ec9 40 }
marcoanewman 0:5af6295f4ec9 41
marcoanewman 0:5af6295f4ec9 42 void CartesianRobot::disable(void)
marcoanewman 0:5af6295f4ec9 43 {
marcoanewman 0:5af6295f4ec9 44 _x_stepper.disable();
marcoanewman 0:5af6295f4ec9 45 _y_stepper.disable();
marcoanewman 0:5af6295f4ec9 46 _z_stepper.disable();
marcoanewman 0:5af6295f4ec9 47 }
marcoanewman 0:5af6295f4ec9 48
marcoanewman 0:5af6295f4ec9 49 bool CartesianRobot::enabled(void)
marcoanewman 0:5af6295f4ec9 50 {
marcoanewman 0:5af6295f4ec9 51 return _x_stepper.enabled() && _y_stepper.enabled() && _z_stepper.enabled();
marcoanewman 0:5af6295f4ec9 52 }
marcoanewman 0:5af6295f4ec9 53
marcoanewman 0:5af6295f4ec9 54 bool CartesianRobot::stopped(void)
marcoanewman 0:5af6295f4ec9 55 {
marcoanewman 0:5af6295f4ec9 56 return _x_stepper.stopped() && _y_stepper.stopped() && _z_stepper.stopped();
marcoanewman 0:5af6295f4ec9 57 }
marcoanewman 0:5af6295f4ec9 58
marcoanewman 0:5af6295f4ec9 59 int CartesianRobot::getXPosition(void){return _x_stepper.getPosition();}
marcoanewman 0:5af6295f4ec9 60 int CartesianRobot::getYPosition(void){return _y_stepper.getPosition();}
marcoanewman 0:5af6295f4ec9 61 int CartesianRobot::getZPosition(void){return _z_stepper.getPosition();}
marcoanewman 0:5af6295f4ec9 62
marcoanewman 0:5af6295f4ec9 63 int CartesianRobot::getXLimit(void){return _x_stepper.getPositionLimit();}
marcoanewman 0:5af6295f4ec9 64 int CartesianRobot::getYLimit(void){return _y_stepper.getPositionLimit();}
marcoanewman 0:5af6295f4ec9 65 int CartesianRobot::getZLimit(void){return _z_stepper.getPositionLimit();}
marcoanewman 0:5af6295f4ec9 66
marcoanewman 0:5af6295f4ec9 67
marcoanewman 0:5af6295f4ec9 68 ////////////////////////////////////////////////////////////////////////////////
marcoanewman 0:5af6295f4ec9 69 // Calibration //
marcoanewman 0:5af6295f4ec9 70 ////////////////////////////////////////////////////////////////////////////////
marcoanewman 0:5af6295f4ec9 71 void CartesianRobot::setOrigin(void)
marcoanewman 0:5af6295f4ec9 72 {
marcoanewman 0:5af6295f4ec9 73 _x_stepper.setPositionZero();
marcoanewman 0:5af6295f4ec9 74 _y_stepper.setPositionZero();
marcoanewman 0:5af6295f4ec9 75 _z_stepper.setPositionZero();
marcoanewman 0:5af6295f4ec9 76 }
marcoanewman 0:5af6295f4ec9 77
marcoanewman 0:5af6295f4ec9 78 void CartesianRobot::setLimits(void)
marcoanewman 0:5af6295f4ec9 79 {
marcoanewman 0:5af6295f4ec9 80 _x_stepper.setPositionLimit();
marcoanewman 0:5af6295f4ec9 81 _y_stepper.setPositionLimit();
marcoanewman 0:5af6295f4ec9 82 _z_stepper.setPositionLimit();
marcoanewman 0:5af6295f4ec9 83 }
marcoanewman 0:5af6295f4ec9 84
marcoanewman 0:5af6295f4ec9 85
marcoanewman 0:5af6295f4ec9 86 ////////////////////////////////////////////////////////////////////////////////
marcoanewman 0:5af6295f4ec9 87 // Movement //
marcoanewman 0:5af6295f4ec9 88 ////////////////////////////////////////////////////////////////////////////////
marcoanewman 0:5af6295f4ec9 89 void CartesianRobot::moveToXYZ(
marcoanewman 0:5af6295f4ec9 90 int x_pos, int y_pos, int z_pos,
marcoanewman 0:5af6295f4ec9 91 float x_speed, float y_speed, float z_speed
marcoanewman 0:5af6295f4ec9 92 ){
marcoanewman 0:5af6295f4ec9 93 if (_buffer_size < BUFFER_MAX_SIZE){
marcoanewman 0:5af6295f4ec9 94 _buffer_mutex.lock();
marcoanewman 0:5af6295f4ec9 95 _x_buffer_pos[_buffer_size] = x_pos;
marcoanewman 0:5af6295f4ec9 96 _y_buffer_pos[_buffer_size] = y_pos;
marcoanewman 0:5af6295f4ec9 97 _z_buffer_pos[_buffer_size] = z_pos;
marcoanewman 0:5af6295f4ec9 98 _x_buffer_speed[_buffer_size] = x_speed;
marcoanewman 0:5af6295f4ec9 99 _y_buffer_speed[_buffer_size] = y_speed;
marcoanewman 0:5af6295f4ec9 100 _z_buffer_speed[_buffer_size] = z_speed;
marcoanewman 0:5af6295f4ec9 101 _buffer_size++;
marcoanewman 0:5af6295f4ec9 102 _buffer_mutex.unlock();
marcoanewman 0:5af6295f4ec9 103 }
marcoanewman 0:5af6295f4ec9 104 }
marcoanewman 0:5af6295f4ec9 105
marcoanewman 0:5af6295f4ec9 106 void CartesianRobot::goToXYZ(int x_pos, int y_pos, int z_pos){
marcoanewman 0:5af6295f4ec9 107 _x_stepper.goTo(x_pos);
marcoanewman 0:5af6295f4ec9 108 _y_stepper.goTo(y_pos);
marcoanewman 0:5af6295f4ec9 109 _z_stepper.goTo(z_pos);
marcoanewman 0:5af6295f4ec9 110 }
marcoanewman 0:5af6295f4ec9 111
marcoanewman 0:5af6295f4ec9 112 void CartesianRobot::moveX(int steps){_x_stepper.move(steps);}
marcoanewman 0:5af6295f4ec9 113 void CartesianRobot::moveY(int steps){_y_stepper.move(steps);}
marcoanewman 0:5af6295f4ec9 114 void CartesianRobot::moveZ(int steps){_z_stepper.move(steps);}
marcoanewman 0:5af6295f4ec9 115
marcoanewman 0:5af6295f4ec9 116
marcoanewman 0:5af6295f4ec9 117 ////////////////////////////////////////////////////////////////////////////////
marcoanewman 0:5af6295f4ec9 118 // Buffer Management //
marcoanewman 0:5af6295f4ec9 119 ////////////////////////////////////////////////////////////////////////////////
marcoanewman 0:5af6295f4ec9 120 void CartesianRobot::startManager(void) {
marcoanewman 0:5af6295f4ec9 121 _thread = new Thread(manager, this);
marcoanewman 0:5af6295f4ec9 122 }
marcoanewman 0:5af6295f4ec9 123
marcoanewman 0:5af6295f4ec9 124 void CartesianRobot::manager(const void *p){
marcoanewman 0:5af6295f4ec9 125 CartesianRobot* self = (CartesianRobot*)p;
marcoanewman 0:5af6295f4ec9 126 int x_pos, y_pos, z_pos;
marcoanewman 0:5af6295f4ec9 127 float x_speed, y_speed, z_speed;
marcoanewman 0:5af6295f4ec9 128 while(1){
marcoanewman 0:5af6295f4ec9 129 if (self->_buffer_size){
marcoanewman 0:5af6295f4ec9 130 x_pos = self->_x_buffer_pos[0];
marcoanewman 0:5af6295f4ec9 131 y_pos = self->_y_buffer_pos[0];
marcoanewman 0:5af6295f4ec9 132 x_speed = self->_x_buffer_speed[0];
marcoanewman 0:5af6295f4ec9 133 y_speed = self->_y_buffer_speed[0];
marcoanewman 0:5af6295f4ec9 134 z_pos = self->_z_buffer_pos[0];
marcoanewman 0:5af6295f4ec9 135 z_speed = self->_z_buffer_speed[0];
marcoanewman 0:5af6295f4ec9 136 while(!self->stopped());
marcoanewman 0:5af6295f4ec9 137 self->setXSpeed(x_speed);
marcoanewman 0:5af6295f4ec9 138 self->setYSpeed(y_speed);
marcoanewman 0:5af6295f4ec9 139 self->setZSpeed(z_speed);
marcoanewman 0:5af6295f4ec9 140 self->goToXYZ(x_pos, y_pos, z_pos);
marcoanewman 0:5af6295f4ec9 141
marcoanewman 0:5af6295f4ec9 142 self->_buffer_mutex.lock();
marcoanewman 0:5af6295f4ec9 143 for (int i=0; i<self->_buffer_size && i<BUFFER_MAX_SIZE-1; i++){
marcoanewman 0:5af6295f4ec9 144 self->_x_buffer_pos[i] = self->_x_buffer_pos[i+1];
marcoanewman 0:5af6295f4ec9 145 self->_y_buffer_pos[i] = self->_y_buffer_pos[i+1];
marcoanewman 0:5af6295f4ec9 146 self->_x_buffer_speed[i] = self->_x_buffer_speed[i+1];
marcoanewman 0:5af6295f4ec9 147 self->_y_buffer_speed[i] = self->_y_buffer_speed[i+1];
marcoanewman 0:5af6295f4ec9 148 self->_z_buffer_pos[i] = self->_z_buffer_pos[i+1];
marcoanewman 0:5af6295f4ec9 149 self->_z_buffer_speed[i] = self->_z_buffer_speed[i+1];
marcoanewman 0:5af6295f4ec9 150 }
marcoanewman 0:5af6295f4ec9 151 self->_buffer_size--;
marcoanewman 0:5af6295f4ec9 152 self->_buffer_mutex.unlock();
marcoanewman 0:5af6295f4ec9 153 }
marcoanewman 0:5af6295f4ec9 154 }
marcoanewman 0:5af6295f4ec9 155 }