Skip to content

GitLab

Commit 1d6c520b authored by Oliver Anthony's avatar Oliver Anthony
Browse files

V04

parents
Hide whitespace changes
Inline Side-by-side
sketch_1_Device_Setup/Pantograph.java 0 → 100644
View file @ 1d6c520b
/**
**********************************************************************************************************************
* @file Pantograph.java
* @author Steve Ding, Colin Gallacher
* @version V3.0.0
* @date 15-January-2021
* @brief Mechanism extension example
**********************************************************************************************************************
* @attention
*
*
**********************************************************************************************************************
*/
import static java.lang.Math.*;
public class Pantograph extends Mechanisms{
private float l, L, d;
private float th1, th2;
private float tau1, tau2;
private float f_x, f_y;
private float x_E, y_E;
private float pi = 3.14159265359f;
private float JT11, JT12, JT21, JT22;
private float gain = 1.0f;
public Pantograph(){
this.l = 0.07f;
this.L = 0.09f;
this.d = 0.0f;
}
public void torqueCalculation(float[] force){
f_x = force[0];
f_y = force[1];
tau1 = JT11*f_x + JT12*f_y;
tau2 = JT21*f_x + JT22*f_y;
tau1 = tau1*gain;
tau2 = tau2*gain;
}
public void forwardKinematics(float[] angles){
float l1 = l;
float l2 = l;
float L1 = L;
float L2 = L;
th1 = pi/180*angles[0];
th2 = pi/180*angles[1];
// Forward Kinematics
float c1 = (float)cos(th1);
float c2 = (float)cos(th2);
float s1 = (float)sin(th1);
float s2 = (float)sin(th2);
float xA = l1*c1;
float yA = l1*s1;
float xB = d+l2*c2;
float yB = l2*s2;
float hx = xB-xA;
float hy = yB-yA;
float hh = (float) pow(hx,2) + (float) pow(hy,2);
float hm = (float)sqrt(hh);
float cB = - ((float) pow(L2,2) - (float) pow(L1,2) - hh) / (2*L1*hm);
float h1x = L1*cB * hx/hm;
float h1y = L1*cB * hy/hm;
float h1h1 = (float) pow(h1x,2) + (float) pow(h1y,2);
float h1m = (float) sqrt(h1h1);
float sB = (float) sqrt(1-pow(cB,2));
float lx = -L1*sB*h1y/h1m;
float ly = L1*sB*h1x/h1m;
float x_P = xA + h1x + lx;
float y_P = yA + h1y + ly;
float phi1 = (float)acos((x_P-l1*c1)/L1);
float phi2 = (float)acos((x_P-d-l2*c2)/L2);
float c11 = (float) cos(phi1);
float s11 =(float) sin(phi1);
float c22= (float) cos(phi2);
float s22 = (float) sin(phi2);
float dn = L1 *(c11 * s22 - c22 * s11);
float eta = (-L1 * c11 * s22 + L1 * c22 * s11 - c1 * l1 * s22 + c22 * l1 * s1) / dn;
float nu = l2 * (c2 * s22 - c22 * s2)/dn;
JT11 = -L1 * eta * s11 - L1 * s11 - l1 * s1;
JT12 = L1 * c11 * eta + L1 * c11 + c1 * l1;
JT21 = -L1 * s11 * nu;
JT22 = L1 * c11 * nu;
x_E = x_P;
y_E = y_P;
}
public void forceCalculation(){
}
public void positionControl(){
}
public void inverseKinematics(){
}
public void set_mechanism_parameters(float[] parameters){
this.l = parameters[0];
this.L = parameters[1];
this.d = parameters[2];
}
public void set_sensor_data(float[] data){
}
public float[] get_coordinate(){
float temp[] = {x_E, y_E};
return temp;
}
public float[] get_torque(){
float temp[] = {tau1, tau2};
return temp;
}
public float[] get_angle(){
float temp[] = {th1, th2};
return temp;
}
}
sketch_1_Device_Setup/sketch_1_Device_Setup.pde 0 → 100644
View file @ 1d6c520b
/**
**********************************************************************************************************************
* @file sketch_1_Device_Setup.pde
* @author Steve Ding, Colin Gallacher
* @version V1.0.0
* @date 08-January-2021
* @brief hAPI device setup and move end effector in 2D environment
**********************************************************************************************************************
* @attention
*
*
**********************************************************************************************************************
*/
/* library imports *****************************************************************************************************/
import processing.serial.*;
import static java.util.concurrent.TimeUnit.*;
import java.util.concurrent.*;
/* end library imports *************************************************************************************************/
/* scheduler definition ************************************************************************************************/
private final ScheduledExecutorService scheduler = Executors.newScheduledThreadPool(1);
/* end scheduler definition ********************************************************************************************/
/* device block definitions ********************************************************************************************/
Board haplyBoard;
Device widgetOne;
Mechanisms pantograph;
byte widgetOneID = 5;
int CW = 0;
int CCW = 1;
boolean rendering_force = false;
/* end device block definition *****************************************************************************************/
/* framerate definition ************************************************************************************************/
long baseFrameRate = 120;
/* end framerate definition ********************************************************************************************/
/* elements definition *************************************************************************************************/
/* Screen and world setup parameters */
float pixelsPerMeter = 4000.0;
/* generic data for a 2DOF device */
/* joint space */
PVector angles = new PVector(0, 0);
PVector torques = new PVector(0, 0);
/* task space */
PVector posEE = new PVector(0, 0);
PVector fEE = new PVector(0, 0);
/* World boundaries reference */
final int worldPixelWidth = 1000;
final int worldPixelHeight = 650;
/* Initialization of virtual tool */
PShape eeAvatar;
/* end elements definition *********************************************************************************************/
/* setup section *******************************************************************************************************/
void setup(){
/* put setup code here, run once: */
/* screen size definition */
size(1000, 650);
/* device setup */
/**
* The board declaration needs to be changed depending on which USB serial port the Haply board is connected.
* In the base example, a connection is setup to the first detected serial device, this parameter can be changed
* to explicitly state the serial port will look like the following for different OS:
*
* windows: haplyBoard = new Board(this, "COM10", 0);
* linux: haplyBoard = new Board(this, "/dev/ttyUSB0", 0);
* mac: haplyBoard = new Board(this, "/dev/cu.usbmodem1411", 0);
*/
haplyBoard = new Board(this, "COM4", 0);
widgetOne = new Device(widgetOneID, haplyBoard);
pantograph = new Pantograph();
widgetOne.set_mechanism(pantograph);
widgetOne.add_actuator(1, CCW, 2);
widgetOne.add_actuator(2, CW, 1);
widgetOne.add_encoder(1, CCW, 241, 10752, 2);
widgetOne.add_encoder(2, CW, -61, 10752, 1);
widgetOne.device_set_parameters();
eeAvatar = createShape(ELLIPSE, 0, 0, 50, 50);
eeAvatar.setFill(color(0));
/* setup framerate speed */
frameRate(baseFrameRate);
/* setup simulation thread to run at 1kHz */
SimulationThread st = new SimulationThread();
scheduler.scheduleAtFixedRate(st, 1, 1, MILLISECONDS);
}
/* end setup section ***************************************************************************************************/
/* draw section ********************************************************************************************************/
void draw(){
/* put graphical code here, runs repeatedly at defined framerate in setup, else default at 60fps: */
background(255);
update_end_effector(posEE.x, posEE.y);
}
/* end draw section ****************************************************************************************************/
/* simulation section **************************************************************************************************/
class SimulationThread implements Runnable{
public void run(){
/* put haptic simulation code here, runs repeatedly at 1kHz as defined in setup */
rendering_force = true;
if(haplyBoard.data_available()){
/* GET END-EFFECTOR STATE (TASK SPACE) */
widgetOne.device_read_data();
angles.set(widgetOne.get_device_angles());
posEE.set(widgetOne.get_device_position(angles.array()));
posEE.set(device_to_graphics(posEE));
}
torques.set(widgetOne.set_device_torques(fEE.array()));
widgetOne.device_write_torques();
rendering_force = false;
}
}
/* end simulation section **********************************************************************************************/
/* helper functions section, place helper functions here ***************************************************************/
void update_end_effector(float xE, float yE){
background(255);
xE = (pixelsPerMeter * xE) + worldPixelWidth/2;
yE = (pixelsPerMeter * yE);
translate(xE, yE);
shape(eeAvatar);
}
PVector device_to_graphics(PVector deviceFrame){
return deviceFrame.set(-deviceFrame.x, deviceFrame.y);
}
/* end helper functions section ****************************************************************************************/
sketch_2_Hello_Wall/Pantograph.java 0 → 100644
View file @ 1d6c520b
/**
**********************************************************************************************************************
* @file Pantograph.java
* @author Steve Ding, Colin Gallacher
* @version V3.0.0
* @date 15-January-2021
* @brief Mechanism extension example
**********************************************************************************************************************
* @attention
*
*
**********************************************************************************************************************
*/
import static java.lang.Math.*;
public class Pantograph extends Mechanisms{
private float l, L, d;
private float th1, th2;
private float tau1, tau2;
private float f_x, f_y;
private float x_E, y_E;
private float pi = 3.14159265359f;
private float JT11, JT12, JT21, JT22;
private float gain = 1.0f;
public Pantograph(){
this.l = 0.07f;
this.L = 0.09f;
this.d = 0.0f;
}
public void torqueCalculation(float[] force){
f_x = force[0];
f_y = force[1];
tau1 = JT11*f_x + JT12*f_y;
tau2 = JT21*f_x + JT22*f_y;
tau1 = tau1*gain;
tau2 = tau2*gain;
}
public void forwardKinematics(float[] angles){
float l1 = l;
float l2 = l;
float L1 = L;
float L2 = L;
th1 = pi/180*angles[0];
th2 = pi/180*angles[1];
// Forward Kinematics
float c1 = (float)cos(th1);
float c2 = (float)cos(th2);
float s1 = (float)sin(th1);
float s2 = (float)sin(th2);
float xA = l1*c1;
float yA = l1*s1;
float xB = d+l2*c2;
float yB = l2*s2;
float hx = xB-xA;
float hy = yB-yA;
float hh = (float) pow(hx,2) + (float) pow(hy,2);
float hm = (float)sqrt(hh);
float cB = - ((float) pow(L2,2) - (float) pow(L1,2) - hh) / (2*L1*hm);
float h1x = L1*cB * hx/hm;
float h1y = L1*cB * hy/hm;
float h1h1 = (float) pow(h1x,2) + (float) pow(h1y,2);
float h1m = (float) sqrt(h1h1);
float sB = (float) sqrt(1-pow(cB,2));
float lx = -L1*sB*h1y/h1m;
float ly = L1*sB*h1x/h1m;
float x_P = xA + h1x + lx;
float y_P = yA + h1y + ly;
float phi1 = (float)acos((x_P-l1*c1)/L1);
float phi2 = (float)acos((x_P-d-l2*c2)/L2);
float c11 = (float) cos(phi1);
float s11 =(float) sin(phi1);
float c22= (float) cos(phi2);
float s22 = (float) sin(phi2);
float dn = L1 *(c11 * s22 - c22 * s11);
float eta = (-L1 * c11 * s22 + L1 * c22 * s11 - c1 * l1 * s22 + c22 * l1 * s1) / dn;
float nu = l2 * (c2 * s22 - c22 * s2)/dn;
JT11 = -L1 * eta * s11 - L1 * s11 - l1 * s1;
JT12 = L1 * c11 * eta + L1 * c11 + c1 * l1;
JT21 = -L1 * s11 * nu;
JT22 = L1 * c11 * nu;
x_E = x_P;
y_E = y_P;
}
public void forceCalculation(){
}
public void positionControl(){
}
public void inverseKinematics(){
}
public void set_mechanism_parameters(float[] parameters){
this.l = parameters[0];
this.L = parameters[1];
this.d = parameters[2];
}
public void set_sensor_data(float[] data){
}
public float[] get_coordinate(){
float temp[] = {x_E, y_E};
return temp;
}
public float[] get_torque(){
float temp[] = {tau1, tau2};
return temp;
}
public float[] get_angle(){
float temp[] = {th1, th2};
return temp;
}
}
sketch_2_Hello_Wall/sketch_2_Hello_Wall.pde 0 → 100644
View file @ 1d6c520b
/**
**********************************************************************************************************************
* @file sketch_2_Hello_Wall.pde
* @author Steve Ding, Colin Gallacher
* @version V3.0.0
* @date 08-January-2021
* @brief Wall haptic example with programmed physics for a haptic wall
**********************************************************************************************************************
* @attention
*
*
**********************************************************************************************************************
*/
/* library imports *****************************************************************************************************/
import processing.serial.*;
import static java.util.concurrent.TimeUnit.*;
import java.util.concurrent.*;
/* end library imports *************************************************************************************************/
/* scheduler definition ************************************************************************************************/
private final ScheduledExecutorService scheduler = Executors.newScheduledThreadPool(1);
/* end scheduler definition ********************************************************************************************/
/* device block definitions ********************************************************************************************/
Board haplyBoard;
Device widgetOne;
Mechanisms pantograph;
byte widgetOneID = 5;
int CW = 0;
int CCW = 1;
boolean renderingForce = false;
/* end device block definition *****************************************************************************************/
/* framerate definition ************************************************************************************************/
long baseFrameRate = 120;
/* end framerate definition ********************************************************************************************/
/* elements definition *************************************************************************************************/
/* Screen and world setup parameters */
float pixelsPerMeter = 4000.0;
float radsPerDegree = 0.01745;
/* pantagraph link parameters in meters */
float l = 0.07;
float L = 0.09;
/* end effector radius in meters */
float rEE = 0.006;
/* virtual wall parameter */
float kWall = 450;
PVector fWall = new PVector(0, 0);
PVector penWall = new PVector(0, 0);
PVector posWall = new PVector(0.01, 0.10);
/* generic data for a 2DOF device */
/* joint space */
PVector angles = new PVector(0, 0);
PVector torques = new PVector(0, 0);
/* task space */
PVector posEE = new PVector(0, 0);
PVector fEE = new PVector(0, 0);
/* device graphical position */
PVector deviceOrigin = new PVector(0, 0);
/* World boundaries reference */
final int worldPixelWidth = 1000;
final int worldPixelHeight = 650;
/* graphical elements */
PShape pGraph, joint, endEffector;
PShape wall;
/* end elements definition *********************************************************************************************/
/* setup section *******************************************************************************************************/
void setup(){
/* put setup code here, run once: */
/* screen size definition */
size(1000, 650);
/* device setup */
/**
* The board declaration needs to be changed depending on which USB serial port the Haply board is connected.
* In the base example, a connection is setup to the first detected serial device, this parameter can be changed
* to explicitly state the serial port will look like the following for different OS:
*
* windows: haplyBoard = new Board(this, "COM10", 0);
* linux: haplyBoard = new Board(this, "/dev/ttyUSB0", 0);
* mac: haplyBoard = new Board(this, "/dev/cu.usbmodem1411", 0);
*/