Hallo Leute,
ich möchte den Code von einem Laserplotter in einen Stiftplotter "umschreiben". Der unterschied liegt nur darin, dass beim Laserplotter der Laser mit einem High Signal (mit einem relais) ein und mit einem Low Signal ausgeschaltet wird. Beim Stiftplotter steuert ein Servo den Stift (absenken und anheben). Die Hardware habe ich fertig nur der Servo macht mir Probleme. Ich bekomme es einfach nicht hin in in den Code zu implementieren ohne dass er irgendwas irres macht 
Mit dem Servobeispiel sweep funktioniert der Servo super.
Hier ist der Code.. Wichtig ist nur M03 M05 M02 und der Setup teil und der darüber..
#include <AFMotor.h>
AF_Stepper motor1(48, 1); //Y-Achse
AF_Stepper motor2(48, 2); //X-Achse
int LASER = 2;
int cutspeed =10;
#define COMMAND_SIZE 128
char aWord[COMMAND_SIZE];
String command;
String command_part[6];
int part;
int last_space;
long serial_count;
long no_data = 0;
float x,y;
char c;
long code;
char buf[10]; // Buffer für Umwandlung String to Float
int x_neu, y_neu;
int x_aktu, y_aktu;
void setup() {
Serial.begin(9600);
x_aktu= 0;
y_aktu= 0;
pinMode(LASER, OUTPUT);
}
void loop()
{
if (Serial.available() > 0) {
// nächstes Zeichen
//====================
c = Serial.read();
no_data = 0;
// newline ist Befehlsende
//==========================
if (c != '\n') {
aWord[serial_count] = c;
serial_count++;
}
}
if (serial_count && (c == '\n')) {
no_data = 0;
c = ' ';
command=aWord;
int length;
part=0;
last_space= 0;
for (int i=0; i < serial_count; i++)
{
if (command.charAt(i) == ' ') {
command_part[part] = command.substring(last_space,i);
last_space=i+1;
if (command_part[part].length() != 0) {
part++;
}
}
}
command_part[part] = command.substring(last_space,serial_count); //letztes Kommando extrahieren
part++;
for (int i=0; i < part; i++){
}
if ((command_part[0] == "G01") || (command_part[0] == "g01") || (command_part[0] == "G1")) {
extract_parameter();
moveTo(x_neu, y_neu,cutspeed);
Serial.println("OK");
}
if ((command_part[0] == "G02") || (command_part[0] == "g02") || (command_part[0] == "G2")) {
extract_parameter();
moveTo(x_neu, y_neu,cutspeed);
Serial.println("OK");
}
if ((command_part[0] == "G03") || (command_part[0] == "g03") || (command_part[0] == "G3")) {
extract_parameter();
moveTo(x_neu, y_neu,cutspeed);
Serial.println("OK");
}
if ((command_part[0] == "G21") || (command_part[0] == "g21")) {
//Nix tun
Serial.println("OK");
}
if ((command_part[0] == "G90") || (command_part[0] == "g90")) {
//Nix tun
Serial.println("OK");
}
if ((command_part[0] == "G0") || (command_part[0] == "g0")) {
extract_parameter();
moveTo(x_neu, y_neu,5);
Serial.println("OK");
}
if ((command_part[0] == "M03") || (command_part[0] == "M3")) {
//Laser an
digitalWrite(LASER, HIGH);
Serial.println("OK");
}
if ((command_part[0] == "M05") || (command_part[0] == "M5")) {
//Laser aus
digitalWrite(LASER, LOW);
Serial.println("OK");
}
if ((command_part[0] == "M02") || (command_part[0] == "M2")) {
//Laser aus
digitalWrite(LASER, LOW);
Serial.println("OK");
}
clear_process_string();
}
}
void line(int x0, int y0, int x1, int y1, int velo)
{
int dx = abs(x1-x0), sx = x0<x1 ? 1 : -1; // 1 Forward -1 Backward
int dy = -abs(y1-y0), sy = y0<y1 ? 1 : -1; //
int err = dx+dy, e2; /* error value e_xy */
for(;;){ /* loop */
if (x0==x1 && y0==y1) break;
e2 = 2*err;
if (e2 > dy) {
err += dy; x0 += sx;
if (sx == 1) motor1.onestep(FORWARD, INTERLEAVE);
else motor1.onestep(BACKWARD, INTERLEAVE);
}
if (e2 < dx) {
err += dx; y0 += sy;
if (sy == 1) motor2.onestep(FORWARD, INTERLEAVE);
else motor2.onestep(BACKWARD, INTERLEAVE);
}
delay (velo);
}
}
void clear_process_string()
{
// init
for (byte i=0; i<COMMAND_SIZE; i++)
aWord[i] = 0;
serial_count = 0;
}
void moveTo(int x1, int y1, int velo)
{
line (x_aktu, y_aktu, x1, y1, velo);
x_aktu=x1;
y_aktu=y1;
}
void extract_parameter()
{
if (command_part[1].startsWith("X")) {
command_part[1] = command_part[1].substring(1, command_part[1].length()+1);
command_part[1].toCharArray(buf,command_part[1].length()+1);
x = atof (buf);
x = x *10;
x_neu = int (x);
}
if (command_part[2].startsWith("Y")) {
command_part[2] = command_part[2].substring(1, command_part[2].length()+1);
command_part[2].toCharArray(buf,command_part[2].length()+1);
y= atof (buf);
y = y *10;
y_neu = int (y);
}
if (command_part[1].startsWith("F")) {
command_part[1] = command_part[1].substring(1, command_part[1].length()+1);
command_part[1].toCharArray(buf,command_part[1].length()+1);
cutspeed = atof (buf);
cutspeed = int (900/cutspeed);
}
}
Danke schon im Vorraus!