Friday, December 23, 2016
Wednesday, December 21, 2016
Monday, December 19, 2016
robbie1-4 dual gyros
#include "I2Cdev.h"
#include "MPU6050.h"
// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
#define XSTEP 54
#define XDIR 55
#define XEN 38
#define YSTEP 60
#define YDIR 61
#define YEN 56
#define ZSTEP 46
#define ZDIR 48
#define ZEN 62
#define E0STEP 26
#define E0DIR 28
#define E0EN 24
#define E1STEP 36
#define E1DIR 34
#define E1EN 30
// class default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for InvenSense evaluation board)
// AD0 high = 0x69
MPU6050 accelgyro;
MPU6050 accelgyro2(0x69); // <-- use for AD0 high
int16_t ax, ay, az;
int16_t gx, gy, gz;
int pushButton1 = 57;
int pushButton2 = 58;
unsigned int t00=0,t01=0;
unsigned int t10=0,t11=0;
int dt1=0,dt2=0;
int buttonState1=false;
int buttonState2=false;
int val1=0,val2=0;
int count1=0;int maxcount1=0;
int count2=0;int maxcount2=0;
int count3=0;int maxcount3=0;
void setup() {
pinMode(pushButton1, INPUT);
pinMode(pushButton2, INPUT);
pinMode(YEN,OUTPUT);
pinMode(YDIR,OUTPUT);
pinMode(YSTEP,OUTPUT);
pinMode(XEN,OUTPUT);
pinMode(XDIR,OUTPUT);
pinMode(XSTEP,OUTPUT);
digitalWrite(XEN,LOW);
digitalWrite(XDIR,LOW);
digitalWrite(XSTEP,LOW);
digitalWrite(YEN,LOW);
digitalWrite(YDIR,LOW);
digitalWrite(YSTEP,LOW);
pinMode(ZEN,OUTPUT);
pinMode(ZDIR,OUTPUT);
pinMode(ZSTEP,OUTPUT);
digitalWrite(ZEN,LOW);
digitalWrite(ZDIR,LOW);
digitalWrite(ZSTEP,LOW);
pinMode(E0EN,OUTPUT);
pinMode(E0DIR,OUTPUT);
pinMode(E0STEP,OUTPUT);
digitalWrite(E0EN,LOW);
digitalWrite(E0DIR,HIGH);
digitalWrite(E0STEP,LOW);
pinMode(E1EN,OUTPUT);
pinMode(E1DIR,OUTPUT);
pinMode(E1STEP,OUTPUT);
digitalWrite(E1EN,LOW);
digitalWrite(E1DIR,LOW);
digitalWrite(E1STEP,LOW);
Fastwire::setup(400, false);
// initialize serial communication
// (38400 chosen because it works as well at 8MHz as it does at 16MHz, but
// it's really up to you depending on your project)
Serial.begin(115200);
//while(!Serial.available());
// initialize device
Serial.println("Initializing I2C devices...");
accelgyro.initialize();
accelgyro2.initialize();
// verify connection
Serial.println("Testing device connections...");
Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
Serial.println(accelgyro2.testConnection() ? "MPU6050 connection 2 successful" : "MPU6050 connection 2 failed");
// use the code below to change accel/gyro offset values
accelgyro.setXGyroOffset(19);
accelgyro.setYGyroOffset(68);
accelgyro.setZGyroOffset(88);
accelgyro.setXAccelOffset(-3914);
accelgyro.setYAccelOffset(210);
accelgyro.setZAccelOffset(2680);
accelgyro2.setXGyroOffset(19);
accelgyro2.setYGyroOffset(68);
accelgyro2.setZGyroOffset(88);
accelgyro2.setXAccelOffset(-3914);
accelgyro2.setYAccelOffset(210);
accelgyro2.setZAccelOffset(2680);
}
uint8_t BUFFER[14];
int state=0;
float angle1=0.0;float offset1=0.0;
float angle2=0.0;float offset2=60.0;
double angleY=0.0;
bool tripped=false;
bool counter_switch1=false;
bool counter_switch2=false;
bool counter_switch3=false;
int mode=5;
String command;
void loop() {
if(Serial){
if(Serial.available()){
command=Serial.readString();
if(command[0]=='M'){
String temp=command.substring(1,command.length());
int firstspace=temp.indexOf(" ");
String one=temp.substring(0,firstspace);
temp=temp.substring(firstspace+1,temp.length());
int secondspace=temp.indexOf(" ");
String two=temp.substring(0,secondspace);
temp=temp.substring(secondspace+1,temp.length());
maxcount1=one.toInt();
maxcount2=two.toInt();
maxcount3=temp.toInt();
if(maxcount1>0){
digitalWrite(XDIR,HIGH);
}
else{
digitalWrite(XDIR,LOW);
}
if(maxcount2>0){
digitalWrite(YDIR,HIGH);
}
else{
digitalWrite(YDIR,LOW);
}
if(maxcount3>0){
digitalWrite(E1DIR,HIGH);
}
else{
digitalWrite(E1DIR,LOW);
}
maxcount1=abs(maxcount1);
maxcount2=abs(maxcount2);
maxcount3=abs(maxcount3);
Serial.print(maxcount1);Serial.print(":");
Serial.print(maxcount2);Serial.print(":");
Serial.println(maxcount3);
counter_switch1=true;
counter_switch2=true;
counter_switch3=true;
count1=count2=count3=0;
}
if(command[0]=='O'){
String temp=command.substring(1,command.length());
int firstspace=temp.indexOf(" ");
String one=temp.substring(0,firstspace);
temp=temp.substring(firstspace+1,temp.length());
int secondspace=temp.indexOf(" ");
String two=temp.substring(0,secondspace);
offset1=(float)one.toInt();
offset2=(float)two.toInt();
Serial.println(offset1);
Serial.println(offset2);
}
else if(command[0]=='m'){
command=command.substring(1,command.length());
mode=command.toInt();
if(mode==0){
digitalWrite(XEN,HIGH);
digitalWrite(YEN,HIGH);
}
else if(mode==1){
digitalWrite(XEN,LOW);
digitalWrite(YEN,LOW);
}
else if(mode==2){
counter_switch1=false;
counter_switch2=false;
counter_switch3=false;
maxcount1=60000;
maxcount2=60000;
maxcount3=60000;
count1=count2=count3=0;
}
else if(mode==3){
}
else if(mode==4){Serial.print(offset2);Serial.println(" Mode 4");}
else if(mode==5){Serial.println(" Mode 5");}
else mode=0;
//Serial.println("Changed Mode...");
}
else if(command[0]=='a'){
offset1+=1.0;
//if(offset1>10.0)offset1=10.0;
}
else if(command[0]=='z'){
offset1-=1.0;
//if(offset1<-10.0)offset1=-10.0;
}
else if(command[0]=='s'){
offset2+=1.0;
//if(offset2>10.0)offset2=10.0;
Serial.println(offset2);
}
else if(command[0]=='x'){
offset2-=1.0;
//if(offset2<-10.0)offset2=-10.0;
Serial.println(offset2);
}
}
}
//////////////////////////////////////////////////////////////////////////////////////
if(mode==0){
tripped=false;
state=I2Cdev::readBytes(0x68,MPU6050_RA_ACCEL_YOUT_H,2, BUFFER);//
//Serial.print(state);
if(state>=0){
ay= (((int16_t)BUFFER[0]) << 8) | BUFFER[1];
}
else{
// Serial.print("FAIL1");
Fastwire::reset();
tripped=true;
// Fastwire::setup(400, false);
}
state=I2Cdev::readBytes(0x68, 0x3F, 2,BUFFER);
if(state>=0){
az= (((int16_t)BUFFER[0]) << 8) | BUFFER[1];
}
else{
// Serial.print("FAIL2");
Fastwire::reset();
tripped=true;
}
// Serial.print(gz); Serial.print(":");
// Serial.print(az); Serial.print(":");
if(!tripped){
angle1=atan2(ay,az)*180.0/M_PI+offset1;
//Serial.print(angle1);Serial.print("\t");
if(angle1>0.0){
digitalWrite(ZDIR,HIGH);
digitalWrite(E0DIR,LOW);
}
else{
digitalWrite(ZDIR,LOW);
digitalWrite(E0DIR,HIGH);
}
if(abs(angle1)<0.75){maxcount3=0;counter_switch3=false;}
else {
counter_switch3=true;
maxcount3=(105.0-10.0*abs(angle1));
if(maxcount3<5)maxcount3=5;
}
}else {counter_switch3=false;maxcount3=0;}
// Serial.print(maxcount3);Serial.println(":");
// for(int i=0;i<10;i++){
if(counter_switch3==true && count3>=maxcount3){
digitalWrite(ZSTEP,HIGH);
digitalWrite(E0STEP,HIGH);
count3=0;
}
delayMicroseconds(20);
digitalWrite(ZSTEP,LOW);
digitalWrite(E0STEP,LOW);
//if(counter_switch3==true)
count3++;
}///////////////////////////////////////////////////////////////////////
else if(mode==1){
val1=digitalRead(pushButton1);
val2=digitalRead(pushButton2);
if(val1==HIGH && buttonState1==false){
buttonState1=true;
t00=micros();
}
else if(val1==LOW && buttonState1==true){
t01=micros();
if(t01>t00)dt1=t01-t00-1600;
buttonState1=false;
if(dt1>0){
digitalWrite(XDIR,HIGH);
}
else{
digitalWrite(XDIR,LOW);
}
if(abs(dt1)<200)maxcount1=1000;
else maxcount1=(1680-3*abs(dt1))/50;
if (maxcount1>=0 && maxcount1<5)maxcount1=5;
if(maxcount1<=-1)maxcount1=1000;
}
if(val2==HIGH && buttonState2==false){
buttonState2=true;
t10=micros();
}
else if(val2==LOW && buttonState2==true){
t11=micros();
if(t11>t10){dt2=t11-t10-1600;}//Serial.println(dt2);}
buttonState2=false;
if(dt2>0){
digitalWrite(YDIR,HIGH);
}
else{
digitalWrite(YDIR,LOW);
}
if(abs(dt2)<200)maxcount2=1000;
else maxcount2=(1680-3*abs(dt2))/50;
if (maxcount2>=0 && maxcount2<5)maxcount2=5;
if(maxcount2<=-1)maxcount2=1000;
}
if(count1>=maxcount1){
digitalWrite(XSTEP,HIGH);
count1=0;
}
if(count2>=maxcount2){
digitalWrite(YSTEP,HIGH);
count2=0;
}
delayMicroseconds(50);
digitalWrite(YSTEP,LOW);
digitalWrite(XSTEP,LOW);
digitalWrite(ZSTEP,LOW);
digitalWrite(E0STEP,LOW);
count1++;
count2++;
}
///////////////////////////////////////////////////////////////////////
else if(mode==2){
if(counter_switch1==true && count1>=maxcount1){
digitalWrite(XSTEP,HIGH);
count1=0;
}
if(counter_switch2==true && count2>=maxcount2){
digitalWrite(YSTEP,HIGH);
count2=0;
}
if(counter_switch3==true && count3>=maxcount3){
digitalWrite(E1STEP,HIGH);
count3=0;
}
delayMicroseconds(50);
digitalWrite(YSTEP,LOW);
digitalWrite(XSTEP,LOW);
digitalWrite(E1STEP,LOW);
count1++;
count2++;
count3++;
}
///////////////////////////////////////////////////////////////////////
else if(mode==3){
tripped=false;
//MPU6050_RA_GYRO_ZOUT_H
state=I2Cdev::readBytes(0x68,MPU6050_RA_GYRO_XOUT_H,2, BUFFER);//
//Serial.print(state);
if(state>=0){
gx= (((int16_t)BUFFER[0]) << 8) | BUFFER[1];
}
else{
//Serial.print("FAIL1");
Fastwire::reset();
tripped=true;
// Fastwire::setup(400, false);
}
if(!tripped && abs(gx)>200)angleY+=double(gx)/(4.0*16384.0);
Serial.println(angleY);
}////////////////////////////////////////////////////////////////////////////////////
else if(mode==4){//////////////////////////////////////////////////////////////////////
tripped=false;
state=I2Cdev::readBytes(0x69,MPU6050_RA_ACCEL_YOUT_H,2, BUFFER);//
//Serial.print(state);
if(state>=0){
ay= (((int16_t)BUFFER[0]) << 8) | BUFFER[1];
}
else{
// Serial.print("FAIL1");
Fastwire::reset();
tripped=true;
// Fastwire::setup(400, false);
}
state=I2Cdev::readBytes(0x69, 0x3F, 2,BUFFER);
if(state>=0){
az= (((int16_t)BUFFER[0]) << 8) | BUFFER[1];
}
else{
// Serial.print("FAIL2");
Fastwire::reset();
tripped=true;
}
// Serial.print(gz); Serial.print(":");
// Serial.print(az); Serial.print(":");
if(!tripped){
angle2=atan2(ay,az)*180.0/M_PI+offset2;
// Serial.print(angle2);Serial.print("\t");
if(angle2>0.0){
digitalWrite(E1DIR,LOW);
}
else{
digitalWrite(E1DIR,HIGH);
}
if(abs(angle2)<0.75){maxcount3=0;counter_switch3=false;}
else {
counter_switch3=true;
maxcount3=(50.0-5.0*abs(angle2));
if(maxcount3<5)maxcount3=5;
}
}else {counter_switch3=false;maxcount3=0;}
// Serial.print(maxcount3);Serial.println(":");
// for(int i=0;i<10;i++){
if(counter_switch3==true && count3>=maxcount3){
digitalWrite(E1STEP,HIGH);
count3=0;
}
delayMicroseconds(50);
digitalWrite(E1STEP,LOW);
//if(counter_switch3==true)
count3++;
}///////////////////////////////////////////////////////////////////////
else if(mode==5){
tripped=false;
state=I2Cdev::readBytes(0x68,MPU6050_RA_ACCEL_YOUT_H,2, BUFFER);//
//Serial.print(state);
if(state>=0){
ay= (((int16_t)BUFFER[0]) << 8) | BUFFER[1];
}
else{
// Serial.print("FAIL1");
Fastwire::reset();
tripped=true;
// Fastwire::setup(400, false);
}
state=I2Cdev::readBytes(0x68, 0x3F, 2,BUFFER);
if(state>=0){
az= (((int16_t)BUFFER[0]) << 8) | BUFFER[1];
}
else{
// Serial.print("FAIL2");
Fastwire::reset();
tripped=true;
}
// Serial.print(gz); Serial.print(":");
// Serial.print(az); Serial.print(":");
if(!tripped){
angle1=atan2(ay,az)*180.0/M_PI+offset1;
//Serial.print(angle1);Serial.print("\t");
if(angle1>0.0){
digitalWrite(ZDIR,HIGH);
digitalWrite(E0DIR,LOW);
}
else{
digitalWrite(ZDIR,LOW);
digitalWrite(E0DIR,HIGH);
}
if(abs(angle1)<0.75){maxcount3=0;counter_switch3=false;}
else {
counter_switch3=true;
maxcount3=(105.0-10.0*abs(angle1));
if(maxcount3<5)maxcount3=5;
}
}else {counter_switch3=false;maxcount3=0;}
tripped=false;
state=I2Cdev::readBytes(0x69,MPU6050_RA_ACCEL_YOUT_H,2, BUFFER);//
//Serial.print(state);
if(state>=0){
ay= (((int16_t)BUFFER[0]) << 8) | BUFFER[1];
}
else{
// Serial.print("FAIL1");
Fastwire::reset();
tripped=true;
// Fastwire::setup(400, false);
}
state=I2Cdev::readBytes(0x69, 0x3F, 2,BUFFER);
if(state>=0){
az= (((int16_t)BUFFER[0]) << 8) | BUFFER[1];
}
else{
// Serial.print("FAIL2");
Fastwire::reset();
tripped=true;
}
// Serial.print(gz); Serial.print(":");
// Serial.print(az); Serial.print(":");
if(!tripped){
angle2=atan2(ay,az)*180.0/M_PI+offset2;
// Serial.print(angle2);Serial.print("\t");
if(angle2>0.0){
digitalWrite(E1DIR,LOW);
}
else{
digitalWrite(E1DIR,HIGH);
}
if(abs(angle2)<0.75){maxcount2=0;counter_switch2=false;}
else {
counter_switch2=true;
maxcount2=(50.0-5.0*abs(angle2));
if(maxcount2<5)maxcount2=5;
}
}else {counter_switch2=false;maxcount2=0;}
if(counter_switch3==true && count3>=maxcount3){
digitalWrite(ZSTEP,HIGH);
digitalWrite(E0STEP,HIGH);
count3=0;
}
if(counter_switch2==true && count2>=maxcount2){
digitalWrite(E1STEP,HIGH);
count2=0;
}
digitalWrite(E1STEP,LOW);
digitalWrite(ZSTEP,LOW);
digitalWrite(E0STEP,LOW);
count3++;
count2++;
}
}
Wednesday, December 7, 2016
Robbie 1-4 six wheels 2WD
http://www.thingiverse.com/thing:1742052
3 variable regulators
4s 3300mA lipo battery
Ramps1.4 board
ATMega2560 board.
PCduino nano3b board running Armbian
32 Gb SDcard
USB 4 port hub
FS-GT2 wireless controller
GY21 MPU6050 gyroscope/accelerometer I2C interface
UC28 12 Volt Projector
5 x A4988 Stepper drivers
Why use a projector?
The main reason robbie has a projector is to help the robot,using a camera and opencv, determine the terrain in front of itself and hence be able to navigate.
The following code is for the ATMEGA2560.
You must use Fastwire for this program to work!! Open I2Cdev.h and uncomment the Fastwire declaration to enable it.
#include "I2Cdev.h"
#include "MPU6050.h"
// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
#define XSTEP 54
#define XDIR 55
#define XEN 38
#define YSTEP 60
#define YDIR 61
#define YEN 56
#define ZSTEP 46
#define ZDIR 48
#define ZEN 62
#define E0STEP 26
#define E0DIR 28
#define E0EN 24
#define E1STEP 36
#define E1DIR 34
#define E1EN 30
// class default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for InvenSense evaluation board)
// AD0 high = 0x69
MPU6050 accelgyro;
//MPU6050 accelgyro(0x69); // <-- use for AD0 high
int16_t ax, ay, az;
int16_t gx, gy, gz;
int pushButton1 = 57;
int pushButton2 = 58;
unsigned int t00=0,t01=0;
unsigned int t10=0,t11=0;
int dt1=0,dt2=0;
int buttonState1=false;
int buttonState2=false;
int val1=0,val2=0;
int count1=0;int maxcount1=0;
int count2=0;int maxcount2=0;
int count3=0;int maxcount3=0;
void setup() {
pinMode(pushButton1, INPUT);
pinMode(pushButton2, INPUT);
pinMode(YEN,OUTPUT);
pinMode(YDIR,OUTPUT);
pinMode(YSTEP,OUTPUT);
pinMode(XEN,OUTPUT);
pinMode(XDIR,OUTPUT);
pinMode(XSTEP,OUTPUT);
digitalWrite(XEN,LOW);
digitalWrite(XDIR,LOW);
digitalWrite(XSTEP,LOW);
digitalWrite(YEN,LOW);
digitalWrite(YDIR,LOW);
digitalWrite(YSTEP,LOW);
pinMode(ZEN,OUTPUT);
pinMode(ZDIR,OUTPUT);
pinMode(ZSTEP,OUTPUT);
digitalWrite(ZEN,LOW);
digitalWrite(ZDIR,LOW);
digitalWrite(ZSTEP,LOW);
pinMode(E0EN,OUTPUT);
pinMode(E0DIR,OUTPUT);
pinMode(E0STEP,OUTPUT);
digitalWrite(E0EN,LOW);
digitalWrite(E0DIR,HIGH);
digitalWrite(E0STEP,LOW);
pinMode(E1EN,OUTPUT);
pinMode(E1DIR,OUTPUT);
pinMode(E1STEP,OUTPUT);
digitalWrite(E1EN,LOW);
digitalWrite(E1DIR,LOW);
digitalWrite(E1STEP,LOW);
Fastwire::setup(800, false);
// initialize serial communication
// (38400 chosen because it works as well at 8MHz as it does at 16MHz, but
// it's really up to you depending on your project)
Serial.begin(115200);
// initialize device
Serial.println("Initializing I2C devices...");
accelgyro.initialize();
// verify connection
Serial.println("Testing device connections...");
Serial.println(accelgyro.testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
// use the code below to change accel/gyro offset values
accelgyro.setXGyroOffset(19);
accelgyro.setYGyroOffset(54);
accelgyro.setZGyroOffset(88);
accelgyro.setXAccelOffset(-3914);
accelgyro.setYAccelOffset(210);
accelgyro.setZAccelOffset(2680);
}
uint8_t BUFFER[14];
int state=0;
float angle1=0.0;
bool tripped=false;
bool counter_switch1=false;
bool counter_switch2=false;
bool counter_switch3=false;
int mode=0;
String command;
void loop() {
if(Serial){
if(Serial.available()){
command=Serial.readString();
if(command[0]=='M'){
String temp=command.substring(1,command.length());
int firstspace=temp.indexOf(" ");
String one=temp.substring(0,firstspace);
temp=temp.substring(firstspace+1,temp.length());
int secondspace=temp.indexOf(" ");
String two=temp.substring(0,secondspace);
temp=temp.substring(secondspace+1,temp.length());
maxcount1=one.toInt();
maxcount2=two.toInt();
maxcount3=temp.toInt();
if(maxcount1>0){
digitalWrite(XDIR,HIGH);
}
else{
digitalWrite(XDIR,LOW);
}
if(maxcount2>0){
digitalWrite(YDIR,HIGH);
}
else{
digitalWrite(YDIR,LOW);
}
if(maxcount3>0){
digitalWrite(E1DIR,HIGH);
}
else{
digitalWrite(E1DIR,LOW);
}
maxcount1=abs(maxcount1);
maxcount2=abs(maxcount2);
maxcount3=abs(maxcount3);
Serial.print(maxcount1);Serial.print(":");
Serial.print(maxcount2);Serial.print(":");
Serial.println(maxcount3);
counter_switch1=true;
counter_switch2=true;
counter_switch3=true;
count1=count2=count3=0;
}
else if(command[0]=='m'){
command=command.substring(1,command.length());
mode=command.toInt();
if(mode==0){
digitalWrite(XEN,HIGH);
digitalWrite(YEN,HIGH);
}
else if(mode==1){
digitalWrite(XEN,LOW);
digitalWrite(YEN,LOW);
}
else if(mode==2){
counter_switch1=false;
counter_switch2=false;
counter_switch3=false;
maxcount1=60000;
maxcount2=60000;
maxcount3=60000;
count1=count2=count3=0;
}
else mode=0;
//Serial.println("Changed Mode...");
}
}
}
if(mode==0){
tripped=false;
//MPU6050_RA_GYRO_ZOUT_H
state=I2Cdev::readBytes(0x68,MPU6050_RA_GYRO_ZOUT_H,2, BUFFER);//
//Serial.print(state);
if(state>=0){
gz= (((int16_t)BUFFER[0]) << 8) | BUFFER[1];
}
else{
//Serial.print("FAIL1");
Fastwire::reset();
// Fastwire::setup(400, false);
}
state=I2Cdev::readBytes(0x68,MPU6050_RA_ACCEL_YOUT_H,2, BUFFER);//
//Serial.print(state);
if(state>=0){
ay= (((int16_t)BUFFER[0]) << 8) | BUFFER[1];
}
else{
// Serial.print("FAIL1");
Fastwire::reset();
tripped=true;
// Fastwire::setup(400, false);
}
state=I2Cdev::readBytes(0x68, 0x3F, 2,BUFFER);
if(state>=0){
az= (((int16_t)BUFFER[0]) << 8) | BUFFER[1];
}
else{
// Serial.print("FAIL2");
Fastwire::reset();
tripped=true;
}
// Serial.print(gz); Serial.print(":");
// Serial.print(az); Serial.print(":");
if(!tripped){
angle1=(angle1+atan2(ay,az)*180.0/M_PI)/2.0;
//Serial.print(angle1);Serial.print("\t");
if(angle1>0.0){
digitalWrite(ZDIR,HIGH);
digitalWrite(E0DIR,LOW);
}
else{
digitalWrite(ZDIR,LOW);
digitalWrite(E0DIR,HIGH);
}
if(abs(angle1)<0.75){maxcount3=0;counter_switch3=false;}
else {
counter_switch3=true;
maxcount3=(105.0-10.0*abs(angle1));
if(maxcount3<5)maxcount3=5;
}
}else {counter_switch3=false;maxcount3=0;}
// Serial.print(maxcount3);Serial.println(":");
// for(int i=0;i<10;i++){
if(counter_switch3==true && count3>=maxcount3){
digitalWrite(ZSTEP,HIGH);
digitalWrite(E0STEP,HIGH);
count3=0;
}
delayMicroseconds(20);
digitalWrite(ZSTEP,LOW);
digitalWrite(E0STEP,LOW);
//if(counter_switch3==true)
count3++;
}///////////////////////////////////////////////////////////////////////
else if(mode==1){
val1=digitalRead(pushButton1);
val2=digitalRead(pushButton2);
if(val1==HIGH && buttonState1==false){
buttonState1=true;
t00=micros();
}
else if(val1==LOW && buttonState1==true){
t01=micros();
if(t01>t00)dt1=t01-t00-1600;
buttonState1=false;
if(dt1>0){
digitalWrite(XDIR,HIGH);
}
else{
digitalWrite(XDIR,LOW);
}
if(abs(dt1)<200)maxcount1=1000;
else maxcount1=(1680-3*abs(dt1))/50;
if (maxcount1>=0 && maxcount1<5)maxcount1=5;
if(maxcount1<=-1)maxcount1=1000;
}
if(val2==HIGH && buttonState2==false){
buttonState2=true;
t10=micros();
}
else if(val2==LOW && buttonState2==true){
t11=micros();
if(t11>t10){dt2=t11-t10-1600;}//Serial.println(dt2);}
buttonState2=false;
if(dt2>0){
digitalWrite(YDIR,HIGH);
}
else{
digitalWrite(YDIR,LOW);
}
if(abs(dt2)<200)maxcount2=1000;
else maxcount2=(1680-3*abs(dt2))/50;
if (maxcount2>=0 && maxcount2<5)maxcount2=5;
if(maxcount2<=-1)maxcount2=1000;
}
if(count1>=maxcount1){
digitalWrite(XSTEP,HIGH);
count1=0;
}
if(count2>=maxcount2){
digitalWrite(YSTEP,HIGH);
count2=0;
}
delayMicroseconds(50);
digitalWrite(YSTEP,LOW);
digitalWrite(XSTEP,LOW);
digitalWrite(ZSTEP,LOW);
digitalWrite(E0STEP,LOW);
count1++;
count2++;
}
///////////////////////////////////////////////////////////////////////
else if(mode==2){
if(counter_switch1==true && count1>=maxcount1){
digitalWrite(XSTEP,HIGH);
count1=0;
}
if(counter_switch2==true && count2>=maxcount2){
digitalWrite(YSTEP,HIGH);
count2=0;
}
if(counter_switch3==true && count3>=maxcount3){
digitalWrite(E1STEP,HIGH);
count3=0;
}
delayMicroseconds(50);
digitalWrite(YSTEP,LOW);
digitalWrite(XSTEP,LOW);
digitalWrite(E1STEP,LOW);
count1++;
count2++;
count3++;
}
}
Subscribe to:
Posts (Atom)