Commit 0e0e967d authored by 42loop's avatar 42loop
Browse files

modified for new inverters, test version

parent b3beec40
......@@ -14,7 +14,8 @@
byte enablepins[]={enableX,enableY,enableZ};
byte startpinsP[]={startXP,startYP,startZP};
byte startpinsM[]={startXM,startYM,startZM};
byte brakepins[]={brakeX,brakeY,brakeZ};
......@@ -41,6 +42,17 @@ int seq=0;
int targets[4][3]={{10000,0,0},{40000,0,0},{5000,0,0},{50000,0,0}};
int targetcount=4;
void randomizetargets()
{
for (int b=0;b<targetcount;b++)
{
int t=random(5000,50000);
targets[b][0]=t;
Serial.println(t);
}
}
void showallspeeds()
{
#ifdef info
......@@ -79,12 +91,12 @@ Serial.begin(115200);
Serial.println("\fonline...");
setbrake(0,false);
setbrake(0,true);
setbrake(1,true);
setbrake(2,true);
hwTimer.begin(heartbeat, 10000); // blinkLED to run every 0.01 seconds
set_ltc();
set_motors();
pos_from_eeprom();
setEncoder(0,pos[0]);
......@@ -99,24 +111,9 @@ void heartbeat()
}
void setbrake(int ax, bool val)
{
digitalWrite(brakepins[ax],not val);
if (val) Serial.print("Setting brake on ax ");
else Serial.print("Release brake on ax ");
Serial.println(ax);
}
void mot_on(int id)
{
digitalWrite(enablepins[id],true);
}
void mot_off(int id)
{
digitalWrite(enablepins[id],false);
}
void getEncoders()
......@@ -132,13 +129,13 @@ for (int l=0;l<max_ax;l++) if (lastpos[l]!=pos[l]) {change=true;};
if (change)
{
print_positions();
//print_positions();
}
}
void print_positions()
{
Serial.print("#");
Serial.print(pos[0]);
Serial.print("\t");
Serial.print(pos[1]);
......@@ -162,16 +159,23 @@ void setEncoder(int ax,long val)
void loop()
{
long t=micros();
setbrake(0,true);
digitalWrite(startpinsP[0],true);
delay(1000);
setbrake(0,false);
digitalWrite(startpinsP[0],false);
delay(1000);
getEncoders();
// getEncoders();
planner();
//planner calls testpos anyway...
//testpos(0);
// planner();
set_ltc();
// set_motors();
check_serial();
// check_serial();
......@@ -196,22 +200,24 @@ void loop()
*
*/
Serial.println("homing");
homing=true;
boolean islimit=false;
//move to minus endstop
mot_on(ax);
mot_onM(ax);
speed[ax]=null_speed-40;
//showallspeeds();
while (!islimit)
{
set_ltc();
set_motors();
delay(1);
islimit=limitM[ax];
}
//move in plus dir a bit
speed[ax]=null_speed+20;
set_ltc();
set_motors();
delay(1000);
//slowly approach switch
......@@ -219,7 +225,7 @@ speed[ax]=null_speed-10;
islimit=false;
while (!islimit)
{
set_ltc();
set_motors();
delay(1);
islimit=limitM[ax];
}
......@@ -231,18 +237,18 @@ delay(1000);
//move to plus endstop
islimit=false;
mot_on(ax);
mot_onP(ax);
speed[ax]=null_speed+40;
while (!islimit)
{
set_ltc();
set_motors();
delay(1);
islimit=limitP[ax];
getEncoders();
}
speed[ax]=null_speed;
set_ltc();
set_motors();
mot_off(0);
delay(1000);
......@@ -251,10 +257,8 @@ pos_to_eeprom();
#ifdef info
Serial.println("homing of ax "+String(ax)+" done");
#endif
homing=false;
}
/*(***************************************************************)*/
......@@ -50,7 +50,16 @@ for (int ax=0;ax<max_ax;ax++)
}
if (limitchanged)
reportinputs();
{
reportinputs();
if (!homing)
{
issequence=false;
Serial.println("ENDSTOP HIT");
for (int b=0;b<max_ax;b++) mot_off(b);
running=false;
}
}
for (int ax=0;ax<max_ax;ax++)
{
......
void check_serial()
{
byte testax=0;
byte testax=2;
while (Serial.available() > 0) {
// get incoming byte:
......@@ -38,20 +38,26 @@ while (Serial.available() > 0) {
isaxcmd=false;
if (cmd=="?") print_positions();
/*
if (cmd=="a") {mot_on(testax);Serial.println("mot_on:"+String(testax));}
else
if (cmd=="b") {mot_off(testax);speed[testax]=null_speed; set_ltc();Serial.println("mot_off:"+String(testax));}
else
*/
if (cmd=="c")
{speed[testax]+=10;
Serial.println(speed[testax]-null_speed);
set_ltc();
set_motors();
}
else
if (cmd=="d")
{speed[testax]-=10;
Serial.println(speed[testax]-null_speed);
set_ltc();}
set_motors();}
else
if (cmd=="h") {home(0);}
......@@ -59,17 +65,17 @@ while (Serial.available() > 0) {
if (cmd=="p") {
target[0]=10000;
goposax(0);
// goposax(0);
}
else
if (cmd=="q") {
target[0]=33000;
goposax(0);
// goposax(0);
}
else
if (cmd=="o") {
target[0]=55000;
goposax(0);
// goposax(0);
}
else
......@@ -80,9 +86,9 @@ while (Serial.available() > 0) {
else
if (cmd=="n")
if (cmd=="r")
{
sak[0]=true;
randomizetargets();
}
else
......@@ -94,7 +100,7 @@ sak[0]=true;
{
Serial.println("sequence ON");
target[0]=targets[0][0];
goposax(0);
// goposax(0);
}
else
{
......@@ -108,4 +114,3 @@ sak[0]=true;
}
}
const int null_speed=2048;
//max speed in both dirs, <2048:
const int max_speed=800;
//wether to use brakes by planner
volatile boolean usebrake[]={true,true,true};
//minimum speeds to leave the spot:
int mindownspeed[]={null_speed-4,null_speed-5,null_speed-5};
int minupspeed[]={null_speed+4,null_speed+5,null_speed+5};
//acceleration:
volatile byte m[]={5,5,5,5};
volatile float speedmultiplier[]={1.0,1.0,1.0};
volatile float speedmultiplier[]={2.0,1.0,1.0};
const int null_speed=2048;
const int hw_max_spd=null_speed+max_speed;
const int hw_min_spd=null_speed-max_speed;
......@@ -24,8 +29,6 @@ int maxupspeed[]={0,0,0,0};
int minpos[]={0,0,0,0};
int maxpos[]={0,0,0,0};
int mindownspeed[]={null_speed-5,null_speed-5,null_speed-5};
int minupspeed[]={null_speed+5,null_speed+5,null_speed+5};
//acceleration / braking states:
// 0: up
......@@ -93,3 +96,5 @@ volatile boolean running=false;
volatile boolean lastrunning=false;
volatile boolean issequence=false;
volatile boolean homing=false;
......@@ -17,13 +17,21 @@ digitalWrite(ltc_clk,LOW);
digitalWrite(ltc_load,HIGH);
pinMode(enableX,OUTPUT);
pinMode(enableY,OUTPUT);
pinMode(enableZ,OUTPUT);
pinMode(startXP,OUTPUT);
pinMode(startYP,OUTPUT);
pinMode(startZP,OUTPUT);
digitalWrite(enableX,LOW);
digitalWrite(enableY,LOW);
digitalWrite(enableZ,LOW);
digitalWrite(startXP,LOW);
digitalWrite(startYP,LOW);
digitalWrite(startZP,LOW);
pinMode(startXM,OUTPUT);
pinMode(startYM,OUTPUT);
pinMode(startZM,OUTPUT);
digitalWrite(startXM,LOW);
digitalWrite(startYM,LOW);
digitalWrite(startZM,LOW);
pinMode(brakeX,OUTPUT);
pinMode(brakeY,OUTPUT);
......@@ -67,4 +75,3 @@ pinMode(led,OUTPUT);
digitalWrite(led,HIGH);
}
//update analog out for the next ax
//update analog out for all ax, set/clear enable for inverter, set/clear brake
void set_ltc()
void set_motors()
{
byte currentmotor=0;
//Serial.print("setltc: ");Serial.println(speed[0]);
......@@ -10,12 +10,26 @@ for (currentmotor=0;currentmotor<max_ax;currentmotor++)
unsigned int val;
//invert values:
if (speed[currentmotor]>null_speed)
val=null_speed-(speed[currentmotor]-null_speed);
else
val=null_speed+(null_speed-speed[currentmotor]);
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(val);
Serial.println(currentmotor,val);
digitalWrite(mux_enable,LOW);
......@@ -48,11 +62,33 @@ delayMicroseconds(400);
digitalWrite(mux_enable,HIGH);
delayMicroseconds(400);
//probably not needed:
//delayMicroseconds(400);
}
}
void mot_onP(int id)
{
digitalWrite(startpinsP[id],true);
}
void mot_onM(int id)
{
digitalWrite(startpinsM[id],true);
}
void mot_off(int id)
{
digitalWrite(startpinsP[id],false);
digitalWrite(startpinsM[id],false);
}
void setbrake(int ax, bool val)
{
digitalWrite(brakepins[ax],not val);
if (val) Serial.print("Setting brake on ax ");
else Serial.print("Release brake on ax ");
Serial.println(ax);
}
......@@ -32,9 +32,12 @@
#define refZ 8 //EXT B,5
//output to 24V, external circuit C
#define enableX 33 //EXT C,1
#define enableY 34 //EXT C,2
#define enableZ 35 //EXT C,3
#define startXP 33 //EXT C,1
#define startYP 34 //EXT C,2
#define startZP 35 //EXT C,3
#define startXM 33 //EXT C,4
#define startYM 34 //EXT C,5
#define startZM 35 //EXT C,6
//brake outputs
......@@ -52,5 +55,3 @@
#define mux_a2 32
void planner()
/*void planner()
{
for (int a=0;a<max_ax;a++)
......@@ -42,13 +42,17 @@ void planner()
if (issequence)
{
seq++;
if (seq==targetcount) {seq=0;}
if (seq==targetcount)
{
randomizetargets();
seq=0;
}
delay(1000);
target[0]=targets[seq][0];
goposax(0);
}
/*else
else
{
if (repeat)
{
......@@ -62,19 +66,20 @@ void planner()
}
else issequence=false;
}*/
}
}
}
lastrunning=running;
}
*/
//set start values for certain ax
/*
void goposax(int ax)
{
#ifdef info
......@@ -174,10 +179,11 @@ Serial.println(speed[ax]);
}
*/
//testing ax pos for needed action:
/*
void testpos(int ax)
{
int rampunit=50;
......@@ -343,4 +349,4 @@ sak[ax]=true;
}
*/
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment