From 6607ec5daf16da40334ff2bd3b0c6fc134a52cd4 Mon Sep 17 00:00:00 2001
From: 42loop <ulf.freyhoff@hfbk-hamburg.de>
Date: Mon, 29 Jun 2020 14:53:20 +0200
Subject: [PATCH] some cleanup
---
geku_teensy.ino | 19 ---------
initpins.ino | 23 ++++-------
motor_out.ino | 102 +++++++++++++++++++++++-------------------------
3 files changed, 56 insertions(+), 88 deletions(-)
diff --git a/geku_teensy.ino b/geku_teensy.ino
index 9f9b30d..ae33cdc 100644
--- a/geku_teensy.ino
+++ b/geku_teensy.ino
@@ -63,25 +63,6 @@ Serial.println();
}
-bool waitready()
-{
- bool servoready=true;
- for (int ax=0;ax<max_ax;ax++)
- {
- if (!machineready[ax])
- {
- servoready=false;
- Serial.print("Servo ");
- Serial.print(ax);
- Serial.println(" not ready !");
- }
- }
- return servoready;
-
-}
-
-
-
void setup()
{
diff --git a/initpins.ino b/initpins.ino
index 925c905..18625e3 100644
--- a/initpins.ino
+++ b/initpins.ino
@@ -1,7 +1,7 @@
void initpins()
{
-
- pinMode(ltc_clk,OUTPUT);
+//ltc DA-converter
+pinMode(ltc_clk,OUTPUT);
pinMode(ltc_data,OUTPUT);
pinMode(ltc_load,OUTPUT);
pinMode(mux_enable,OUTPUT);
@@ -16,7 +16,7 @@ digitalWrite(mux_a2,LOW);
digitalWrite(ltc_clk,LOW);
digitalWrite(ltc_load,HIGH);
-
+//inverter start plus direction
pinMode(startXP,OUTPUT);
pinMode(startYP,OUTPUT);
pinMode(startZP,OUTPUT);
@@ -25,6 +25,7 @@ digitalWrite(startXP,LOW);
digitalWrite(startYP,LOW);
digitalWrite(startZP,LOW);
+//inverter start minus direction
pinMode(startXM,OUTPUT);
pinMode(startYM,OUTPUT);
pinMode(startZM,OUTPUT);
@@ -33,6 +34,7 @@ digitalWrite(startXM,LOW);
digitalWrite(startYM,LOW);
digitalWrite(startZM,LOW);
+//brake outputs
pinMode(brakeX,OUTPUT);
pinMode(brakeY,OUTPUT);
pinMode(brakeZ,OUTPUT);
@@ -41,7 +43,7 @@ digitalWrite(brakeX,LOW);
digitalWrite(brakeY,LOW);
digitalWrite(brakeZ,LOW);
-
+//limit and ref inputs
pinMode(limitXPpin,INPUT);
pinMode(limitXMpin,INPUT);
pinMode(limitYPpin,INPUT);
@@ -52,12 +54,6 @@ pinMode(refX,INPUT);
pinMode(refY,INPUT);
pinMode(refZ,INPUT);
-/*
-
-pinMode(readyXpin,INPUT);
-pinMode(readyYpin,INPUT);
-pinMode(readyZpin,INPUT);
-*/
digitalWrite(limitXPpin,HIGH);
digitalWrite(limitXMpin,HIGH);
@@ -69,13 +65,8 @@ digitalWrite(limitZMpin,HIGH);
digitalWrite(refX,HIGH);
digitalWrite(refY,HIGH);
digitalWrite(refZ,HIGH);
-/*
-digitalWrite(readyXpin,HIGH);
-digitalWrite(readyYpin,HIGH);
-digitalWrite(readyZpin,HIGH);
-*/
-pinMode(led,OUTPUT);
+pinMode(led,OUTPUT);
digitalWrite(led,HIGH);
}
diff --git a/motor_out.ino b/motor_out.ino
index f5de0d4..2673ce9 100644
--- a/motor_out.ino
+++ b/motor_out.ino
@@ -2,70 +2,66 @@
void set_motors()
{
-byte currentmotor=0;
-//Serial.print("setltc: ");Serial.println(speed[0]);
-for (currentmotor=0;currentmotor<max_ax;currentmotor++)
-{
-
-unsigned int val;
-
-//invert values:
-if (speed[currentmotor]>null_speed) //plus
- {
- val=speed[currentmotor]-null_speed;
- mot_onP(currentmotor);
- setbrake(currentmotor,false);
- }
-else if (speed[currentmotor]<null_speed) //minus
+ byte currentmotor=0;
+ for (currentmotor=0;currentmotor<max_ax;currentmotor++)
{
- val=abs(speed[currentmotor]-null_speed);
- mot_onM(currentmotor);
- setbrake(currentmotor,false);
- }
-else
-{
- val=0; //stop
- mot_off(currentmotor);
- setbrake(currentmotor,true);
-}
-
-Serial.println(currentmotor,val);
-
-digitalWrite(mux_enable,LOW);
-
-
-if ((currentmotor & 1) !=0) digitalWrite(mux_a0,HIGH); else digitalWrite(mux_a0,LOW);
-if ((currentmotor & 2) !=0) digitalWrite(mux_a1,HIGH); else digitalWrite(mux_a1,LOW);
-digitalWrite(mux_a2,LOW);
-
+ unsigned int val;
+
+//adapt values: former 0 was max minus speed, null_speed was stop, 4095 was max plus speed
+ if (speed[currentmotor]>null_speed) //plus
+ {
+ val=speed[currentmotor]-null_speed;
+ mot_onP(currentmotor);
+ setbrake(currentmotor,false);
+ }
+ else if (speed[currentmotor]<null_speed) //minus
+ {
+ val=abs(speed[currentmotor]-null_speed);
+ mot_onM(currentmotor);
+ setbrake(currentmotor,false);
+ }
+ else
+ {
+ val=0; //stop
+ mot_off(currentmotor);
+ setbrake(currentmotor,true);
+ }
+
+ Serial.println(currentmotor,val);
+
+ digitalWrite(mux_enable,LOW);
+
+
+ if ((currentmotor & 1) !=0) digitalWrite(mux_a0,HIGH); else digitalWrite(mux_a0,LOW);
+ if ((currentmotor & 2) !=0) digitalWrite(mux_a1,HIGH); else digitalWrite(mux_a1,LOW);
+ digitalWrite(mux_a2,LOW);
- for (int i=12; i>0; i--) {
- if ((val & (1 << (i-1)))>0) {
- digitalWrite(ltc_data,HIGH);
- }
- else {
- digitalWrite(ltc_data,LOW);
- }
+ for (int i=12; i>0; i--) {
+ if ((val & (1 << (i-1)))>0)
+ digitalWrite(ltc_data,HIGH);
+ else
+ digitalWrite(ltc_data,LOW);
+
+ delayMicroseconds(1);
+ digitalWrite(ltc_clk,HIGH);
+ delayMicroseconds(1);
+ digitalWrite(ltc_clk,LOW);
+ delayMicroseconds(1);
+ }
+ digitalWrite(ltc_load,LOW);
delayMicroseconds(1);
- digitalWrite(ltc_clk,HIGH);
- delayMicroseconds(1);
- digitalWrite(ltc_clk,LOW);
- delayMicroseconds(1);
- }
- digitalWrite(ltc_load,LOW);
- delayMicroseconds(1);
- digitalWrite(ltc_load,HIGH);
+ digitalWrite(ltc_load,HIGH);
-delayMicroseconds(400);
+ delayMicroseconds(400);
-digitalWrite(mux_enable,HIGH);
+ digitalWrite(mux_enable,HIGH);
//probably not needed:
//delayMicroseconds(400);
-}
+ }
}
--
GitLab