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++;
}
}
Subscribe to:
Post Comments (Atom)
No comments:
Post a Comment