Codes Arduino
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));
}