#include <MPU6050_tockn.h>
#include <Wire.h>
int Left_button =10 ; // trim Left switch D10
int Right_button =11; // trim right Switch D11
int SwingRange = 0;// Pot Angle move distance adjuster
int SwingRange2 = 20; // Hold swing range for on and off switch
int once = 0;// Toggle switch
int Push_on = 0;// Toggle switch
int Enable_Motor = 12; //Enable pin motor to ground
int smDirectionPin = 8; //Direction pin 4 nano
int smStepPin = 9; //Stepper pin pin 5 nano
int Motor_clear =0; // input to give tim for the motor to power up with out step inputs
int Move=0;// Slider move distance
int angle = 0;
float angleX = 0; // Variable for Gyro input
int angleX_int= 0 ; // interger of angle X
int angleX_Last = 0;// Storage for the last angle read
int angleX_dif = 0; // The amout of motor move after every cycle
int ABS_POS = 0 ; // Absolute stepper posistion
int Timer =0 ;
int Count = 0 ; // switch positoin place holder
int X_step_distance =0; // Steps the motor moves
MPU6050 mpu6050(Wire);
unsigned long currentMillis = 0;
long previousMillis = 0; // will store last time thru mills
long interval = 1000;
char Incoming_value = 0;
void setup() {
Serial.begin(9600);
Wire.begin();
mpu6050.begin();
mpu6050.calcGyroOffsets(true);
pinMode (Right_button, OUTPUT);
pinMode(smDirectionPin, OUTPUT);
pinMode(smStepPin, OUTPUT);
pinMode(Enable_Motor, OUTPUT); // Motor Grount input Digital pin 12
digitalWrite(Right_button ,HIGH);// LED off
}
void loop() {
// Serial.println(Incoming_value);//Incoming_value
if(Serial.available() > 0)
{
Incoming_value = Serial.read(); //Get values form Phone
if(Incoming_value == '8'){
Left_button= 1;
digitalWrite(Enable_Motor, HIGH);}// Enable motor
else if(Incoming_value == '7')
{ Left_button=2;
digitalWrite(Enable_Motor, LOW);}// Disable motor
if (Incoming_value > 2000 && Incoming_value < 3000) ;
SwingRange = Incoming_value - 2000; //Romove the leader Value
if (Incoming_value > 3000 && Incoming_value < 4000) ;
int Offset_level = Incoming_value - 3000; //Romove the leader Value
}
if (Incoming_value > 3000 && Incoming_value < 4000){
if (Incoming_value > 3050);
Move = (Incoming_value-3050) - Move ;}
else if (Incoming_value < 3050) ;
Move = (Incoming_value-3050)- Move;
Serial.print (Incoming_value);//(angleX_int);
Serial.print('\n');
if (Left_button == 1 && once ==0&& Push_on== 0)// Toggle switch Range only changes after on off cycle
{
once = 1;
int potValue = analogRead(A2); // Read potentiometer value Use this to adjust Responce Distance of the motor trim
int SwingRange = map(potValue, 0, 1023, 20 , 200); // Map the potentiometer value from 40 to 100 Multipier for distance move of motor
SwingRange2 = SwingRange;
ABS_POS=0;
}
if (Left_button == 2 && once ==1&& Push_on== 0)// Toggle switch
{
once = 0;
Push_on = 1;
digitalWrite(Enable_Motor, HIGH);
digitalWrite(Right_button ,LOW); // LED off motor not enables
}
if (Left_button == 1&& once ==0&& Push_on==1)// Toggle switch
{
once = 1;
}
if (Left_button == 2 && once ==1&& Push_on==1)// Toggle switch
{
once = 0;
Push_on = 0;
digitalWrite(Enable_Motor, LOW);// Test of port ground signal for the motor enable
Motor_clear = 2000;
digitalWrite(Right_button ,HIGH);// LED on Motor Enabled
}
digitalWrite(3, LOW); // sets the digital pin 13 off
//int potValueTrim = analogRead(A1); // Read potentiometer value Use this to set Level add or subtracts wing level
//int Offset_level = map(potValueTrim, 0, 1023, -75 , 75); // Map the potentiometer value from -100 to 100
mpu6050.update();
currentMillis = millis();
if(currentMillis - previousMillis > interval)// Slows the down
{
angleX = mpu6050.getAngleX();
angleX = angleX *10; // Multipier to get higher presision angles move deciaml over one place
// angleX = angleX + Offset_level; // Pot adjustment to change wings level
angleX_int = angleX ; // angleX_int Changes angle X from Float to integer
//Serial.println(angleX_int);
//Serial.println(ABS_POS);
previousMillis = currentMillis;
if(angleX_int >= 20 & ABS_POS < 400)
{ X_step_distance = 25;}
if(angleX_int <= -20 & ABS_POS > -400)
{ X_step_distance = -25;}
}
ABS_POS = ABS_POS + X_step_distance; // keeps the absolute location of motor inputs
if (ABS_POS == 400 || ABS_POS == -400 )
{ X_step_distance = 0;} // sets X_step_distance to zero to hold position
/*Here we are calling the rotate function to turn the stepper motor*/
rotate(X_step_distance*SwingRange2, 2.7); //2.7 The motor rotates X_step_distance steps speed of 0.5 (slow 0.1)SwingRange was at 120 position
// step speed relation ship to high of one of them and program will not run
}
/The rotate function turns the stepper motor. Tt accepts two arguments: 'steps' and 'speed'/
void rotate(int steps, float speed){
/This section looks at the 'steps' argument and stores 'HIGH' in the 'direction' variable if /
/'steps' contains a positive number and 'LOW' if it contains a negative./
int direction;
X_step_distance= X_step_distance + Move; //Adds trim positon to steps
if (X_step_distance>0){
direction = HIGH;}
else{
direction = LOW; }
X_step_distance = 0; //Clears x Steps after it has ben set
// speed = 1/speed * 1; //Calculating speed Not used any more
steps = abs(steps); //Stores the absolute value of the content in 'steps' back into the 'steps' variable
digitalWrite(smDirectionPin, direction); //Writes the direction (from our if statement above), to the EasyDriver DIR pin
/Steppin'/
for (int i = 0; i < steps; i++){
digitalWrite(smStepPin, HIGH);
delayMicroseconds(15);// was speed
digitalWrite(smStepPin, LOW);
delayMicroseconds(15);
}
}
