— ENSCI – FabLab/FabFlex

Version des codes Alpha (encore pas mal de bugs)

#include <Stepper.h>

#define RAZ 12

#define PROXY_SENSOR 0

const int mem[29] = { // 0-29cm

621, //0

603, //1

594, //2

585, //3

576, //4

552, //5

525, //6

478, //7

417, //8

367, //9

335, //10

280, //11

267, //12

230, //13

210, //14

190, //15

173, //16

157, //17

144, //18

130, //19

120, //20

112, //21

100, //22

95, //23

85, //24

77, //25

70, //26

60, //27

58  //28

};

unsigned long val = 0;

int res = 0; // resultat

int cnt = 0;

// Les X et Y filtrÈs de l’image prÈcÈdente

int ofx;

// Les X et Y filtrÈs

int fx;

// La cible ‡ atteindre

int nx;

// Vitesse d’Èvolution

float sx;

// damp = frictions (1 = pas de frictions)

const float damp = 0.3;

// k = konstante de vitesse du filtre de mouvement

const float k = 0.3;

/// 4460x 180°

// 2270y  360°

// initialize of the Stepper library:

Stepper tete(200, 2,3, 4,5);

Stepper rouleau(200, 6, 7,8,9);

//Stepper rouleau(400, 2,3, 4,5);

// stock position (actuelle) de la pointe sur le matériau

int positionX = 0;

int positionY = 0;

// position voulue

int targetX = 0;

int targetY = 0;

// réception depuis le port série

char sInput[5];

int serInputByte = 0;

// capteur si nous sommes arrivés

boolean isArrived = false;

void setup() {

// set the motor speed

tete.setSpeed(45);

rouleau.setSpeed(45);

// Initialize the Serial port:

Serial.begin(9600);

ofx = fx = 0;

sx = 0;

val = 0;

for (int i=0; i<10; i++) {

val+=analogRead(0);

delayMicroseconds(360);

}

val = val/10;

// raz();

}

void loop() {

if (Serial.available()>0) {

char val = Serial.read();

if ((val!=’x’) && (val!=’y’)) {

sInput[serInputByte] = val;

if (serInputByte<5) {

serInputByte++;

}

else {

serInputByte = 0;

Serial.println(« ERROR MORE THAN 4 BYTES IN COMMUNICATION! »);

}

}

else {

int number = atoi(sInput);

if (val==’x’) targetX = number;

else

targetY = number;

isArrived = false;

serInputByte = 0;

sInput[0] = 0;

sInput[1] = 0;

sInput[2] = 0;

sInput[3] = 0;

sInput[4] = 0;

}

}

/// multitache pilotage des moteurs

int xTarg = targetX-positionX;

int yTarg = targetY-positionY;

if (xTarg!=0) {

if (xTarg>0) { // avance si plus petit

tete.step(1);

positionX++;

}

else { /// recule si plus grand

tete.step(-1);

positionX–;

}

}

if (yTarg!=0) {

if (yTarg>0) {

rouleau.step(1);

positionY++;

}

else {

rouleau.step(-1);

positionY–;

}

}

/// sync avec l’ordi, ça y est je suis arrivé

if ((xTarg==0)&&(yTarg==0)) {

if (isArrived!=true) {

//  Serial.println(‘o’);

readAndSendSensorData();

}

isArrived = true;

}

/////////

if (cnt<20) {

cnt++;

}

else {

val = 0;

for (int i=0; i<10; i++) {

val+=analogRead(0);

delayMicroseconds(360);

}

val = val/10;

nx = val;

cnt = 0;

res = getMM();

//Serial.println(res);

sx = 0;

}

sx += (nx – fx) * k;

sx *= damp;

fx += sx;

ofx = fx;

//  Serial.println(proxy); /// gimme height info

}

///1170

void readAndSendSensorData() {

Serial.println(res);

}

int getMM() {

int minA = 0;

int maxB = 0;

int minCm = 0;

int maxCm = 0;

for (int i=0; i<29; i++) {

int t = mem[i];

if (t<fx) {

maxB = t;

if (i>0) {

minA = mem[i-1];

minCm = i-1;

maxCm = i;

}

i = 29;

}

}

float result = mapF(fx, minA, maxB, minCm, maxCm);

return int(result*10.0);

}

float mapF(float value,

float istart, float istop,

float ostart, float ostop) {

return ostart + (ostop – ostart) * ((value – istart) / (istop – istart));

}

Version des codes Alpha (encore pas mal de bugs)

import processing.serial.*;

Serial myPort;

String numScanner = «  »;

/// plateau

final int numberOfSteps = 40;

final int maxStepArduinoPlateau = 2270;

final int stepToSendPlateau = maxStepArduinoPlateau/numberOfSteps;

final int maxValueToTouchZero = 240;

int currentStep = 0;

/// head

final int numberOfHeightMeasures = 40;

final int maxStepArduinoHead = 4460;

final int stepToSendHead = maxStepArduinoHead/numberOfHeightMeasures;

int currentHeightStep = 0;

boolean stateLeftToRght = true;

boolean go = false;

boolean goPlateauTurn = false;

boolean goAndBack = true;

boolean finishHim = false;

/// x,y,z

float measuresXYZ[][][] = new float[numberOfSteps][numberOfHeightMeasures][3];

void setup() {

size(800,800,P3D);

// List all the available serial ports:

println(Serial.list());

// I know that the first port in the serial list on my mac

// is always my  Keyspan adaptor, so I open Serial.list()[0].

// Open whatever port is the one you’re using.

myPort = new Serial(this, Serial.list()[0], 9600);

// prepareLists();

}

void draw() {

int input = 0;

if (finishHim!=true) {

String myString = «  »;

while (myPort.available() > 0) {

myString = myPort.readStringUntil(10);

if (myString != null) {

String[] a = splitTokens(myString);

input = int(a[0]);

if (input>0) go = true;

// println(input);

}

}

}

background(30);

///iterate scanner

noStroke();

if (go) {

if (goAndBack) {

if (currentHeightStep<numberOfHeightMeasures-1) {

currentHeightStep++;

}

else {

goPlateauTurn = true;

goAndBack = false;

}

}

else {

if (currentHeightStep>0) {

currentHeightStep–;

}

else {

goPlateauTurn = true;

goAndBack = !goAndBack;

}

}

if ((currentStep<numberOfSteps-1) && (goPlateauTurn==true)) {

currentStep++;

goPlateauTurn = false;

}

else {

if (currentStep==numberOfSteps-1)

finishHim = true;

}

// give info

String xStr = str(currentHeightStep*stepToSendHead) + « x »;

String yStr = str(currentStep*stepToSendPlateau) + « y »;

//   println(xStr +  »  » + yStr);

myPort.write(xStr);

myPort.write(yStr);

println(currentStep + « plateau   » + currentHeightStep +  » head »);

//println(input);

//calculate angles

float currentStepAngle = (TWO_PI/numberOfSteps) * float(currentStep); //plateau

float currentHeightAngle = (PI/numberOfHeightMeasures) * float(currentHeightStep); ///head

// get coordinates

float z = calculateZ(input, currentHeightAngle);

float x = calculateDistanceFromCenter(input, currentHeightAngle);

float y = 0.0;

// rotate coordinates in to the good angle, Z is constant

float xp = sin(currentStepAngle)*x;

float yp = cos(currentStepAngle)*x;

// put values into the memory

measuresXYZ[currentStep][currentHeightStep][0] = xp;

measuresXYZ[currentStep][currentHeightStep][1] = yp;

measuresXYZ[currentStep][currentHeightStep][2] = z;

go = false;

}

/// RENDER

/// view transformations

rotateX(0.8);

translate(width/2,0,-height/2);

fill(255);

for (int i = 0; i<numberOfSteps; i++) {

for (int j = 0; j<numberOfHeightMeasures; j++) {

pushMatrix();

translate(measuresXYZ[i][j][0], measuresXYZ[i][j][1], measuresXYZ[i][j][2]);

ellipse(0,0, 5,5);

popMatrix();

}

}

}

float calculateZ(int in, float angle) {

int c = maxValueToTouchZero-in;

return float(c)*sin(angle);

}

float calculateDistanceFromCenter(int in, float angle) {

int c = in-maxValueToTouchZero;

return float(c)*cos(angle);

}

void prepareLists() {

for (currentStep = 0; currentStep<numberOfSteps; currentStep++) {

for (currentHeightStep = 0; currentHeightStep<numberOfHeightMeasures; currentHeightStep++) {

//calculate angles

float currentStepAngle = (TWO_PI/numberOfSteps) * float(currentStep); //plateau

float currentHeightAngle = (PI/numberOfHeightMeasures) * float(currentHeightStep); ///head

// get coordinates

float z = calculateZ(100, currentHeightAngle);

float x = calculateDistanceFromCenter(100, currentHeightAngle);

float y = 0.0;

// rotate coordinates in to the good angle, Z is constant

float xp = sin(currentStepAngle)*x;

float yp = cos(currentStepAngle)*x;

// put values into the memory

measuresXYZ[currentStep][currentHeightStep][0] = xp;

measuresXYZ[currentStep][currentHeightStep][1] = yp;

measuresXYZ[currentStep][currentHeightStep][2] = z;

}

}

}