#include <SPI.h>
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <AccelStepper.h>   //程序使用AccelStepper库
//屏幕相关参数
#define SCREEN_WIDTH 128   // OLED 屏幕宽度像素点个数
#define SCREEN_HEIGHT 64   // OLED 屏幕高度像素点个数
#define OLED_RESET 12      // 复位按键
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);
//摇杆相关参数
#define Xaxis_PIN A0      //摇杆X轴方向引脚A0
#define Yaxis_PIN A1      //摇杆Y轴方向引脚A1
#define Key_PIN 13        //摇杆中间按钮引脚13
//电机相关参数
#define enablePin  8    // 使能引脚（低电平有效）
#define XdirPin  2      // x方向控制引脚
#define XstepPin  3     // x步进控制引脚
#define AdirPin  4      // A方向控制引脚
#define AstepPin  5     // A步进控制引脚
AccelStepper stepperX(1,XstepPin,XdirPin);//建立步进电机对象X
AccelStepper stepperA(1,AstepPin,AdirPin);//建立步进电机对象A
float XSpeed,  ASpeed ;
//相关变量
int Xval ,Yval , Cursor = 0 ;  //摇杆X,A轴值,光标位置值
unsigned char KeyVal = 0; //摇杆中间按钮
float Xstart_val , Xend_val ,  Astart_val , Aend_val , Time , MaxSpeed = 1000.0 ;
//定义滑轨X起始值/终点值,定义滑轨A起始值/终点值,定义时间

void setup() {
 Serial.begin(9600);
 //Wire.begin(); // 初始化Wire库
 display.begin(SSD1306_SWITCHCAPVCC, 0x3C);// 初始化OLED并设置其IIC地址为 0x3D（有些为0x3C）
 pinMode(Key_PIN , INPUT);
 pinMode(enablePin , OUTPUT);// 使能引脚为输出模式
 digitalWrite(enablePin , HIGH); //初始电机不进入工作状态

 XSpeed=0.0;
 stepperX.setMaxSpeed(MaxSpeed);     // 设置电机最大速度3
 stepperX.setAcceleration(200);  // 设置电机加速度
 stepperX.setSpeed(XSpeed);

 ASpeed=0.0;
 stepperA.setMaxSpeed(MaxSpeed);     // 设置电机最大速度 
 stepperA.setAcceleration(200);  // 设置电机加速度
 stepperA.setSpeed(XSpeed);
 //屏幕初始化

}

void loop(){
 KeyVal = digitalRead(Key_PIN);//读取摇杆中间按钮值，未按下是低电平
 Xval = analogRead(Xaxis_PIN);//读取摇杆X轴方向引脚A0的值，储存到摇杆X轴值中
 Yval = analogRead(Yaxis_PIN);//读取摇杆Y轴方向引脚A1的值，储存到摇杆Y轴值中
 //Serial.println(Xval);
 //Serial.println(Yval);
 showcursor();
 if(Cursor==0){
   if(Xval<482){
     XSpeed = (Xval - 482.0) * MaxSpeed / 482.0;
     stepperX.setSpeed(XSpeed);
     Serial.println(XSpeed);
    } else if(Xval>542.0){
     XSpeed = (Xval - 542.0) * MaxSpeed / (1023.0-542.0);
     stepperX.setSpeed(XSpeed);
     Serial.println(XSpeed);
    } 
    else stepperX.setSpeed(0);
   stepperX.run();
   display.fillRect(80,14+Cursor*9,35,10,SSD1306_BLACK);//擦除当前数字
   display.setCursor(80,16+Cursor*9);
   display.print(stepperX.currentPosition());
   display.print("mm");
   display.display(); 
  }
 }


//光标函数
void showcursor( ){
 display.drawLine(3,24+Cursor*9,124,24+Cursor*9,SSD1306_BLACK);//擦除当前光标
 if (keyScan()==3) {  // 摇杆Y轴向下
     Cursor++;
   if (Cursor>4) Cursor=0;
  }else if (keyScan()==4) {  // 摇杆Y轴向上
   Cursor--;
   if (Cursor<0) Cursor=4;
  }
 display.drawLine(3,24+Cursor*9,124,24+Cursor*9,SSD1306_WHITE);
 display.display();
}

//按键扫描函数
int keyScan(void){
 if (!KeyVal && ((Xval <= 10) || (Xval >= 1010) || (Yval <= 10) || (Yval >= 1010))){
    delay(200);
   if (Xval <= 10)return 1;//摇杆X轴向右返回1
   else if (Xval >= 1010)return 2;//摇杆X轴向左返回2
   else if (Yval <= 10)return 3;//摇杆Y轴向下返回3
   else if (Yval >= 1010)return 4;//摇杆Y轴向上返回4
  }else if ((Xval > 10) && (Xval < 1010) && (Yval > 10) && (Yval < 1010))KeyVal = 1;
   return 0;//摇杆归正，按下按键返回0
   
}
///////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////
#include <Wire.h>
#include <Adafruit_GFX.h>
#include <Adafruit_SSD1306.h>
#include <AccelStepper.h>
#include <MultiStepper.h>
#include "bitmap.h"

#define limitSwitch 11//限位开关
//屏幕相关参数
#define SCREEN_WIDTH 128   // OLED 屏幕宽度像素点个数
#define SCREEN_HEIGHT 64   // OLED 屏幕高度像素点个数
#define OLED_RESET 12      // 复位按键
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);
//摇杆相关参数
#define XPIN A0      //摇杆X轴(X)方向引脚A0
#define YPIN A1      //摇杆Y轴(A)方向引脚A1
#define KPIN 2        //摇杆中间按钮引脚13
//电机相关参数
#define enablePin 8    // 使能引脚（低电平有效）
#define XdirPin   4    // x方向控制引脚
#define XstepPin  5     // x步进控制引脚
#define AdirPin   5      // A方向控制引脚
#define AstepPin  6     // A步进控制引脚

AccelStepper stepperX(1,XstepPin,XdirPin);//建立步进电机对象X
AccelStepper stepperA(1,AstepPin,AdirPin);//建立步进电机对象A
MultiStepper StepperControl;//创建多电机控制

//相关变量
long gotoposition[2]; //建立到达位置的一维数组
volatile long XStartPoint=0;
volatile long XEndPoint=0;
volatile long AStartAngle=0;
volatile long AEndAngle=0;  
volatile long totaldistance=0;
int Cursor=0;// 按键按下次数
int Xval ,Yval ;

unsigned long Key0=0;//按键消抖参数
unsigned long rotary0=0;//

int setTime=0;//设置总时间
float motorspeed;//电机速度
float timeinsec;//秒钟
float timeinmins;//分钟

volatile boolean TurnDetected;//检测编码器旋转
volatile boolean rotationdirection;//转动方向

//按键消抖
void Switch()  
{
 if(millis()-Key0>500)//按下时间大于500毫秒
 {
 Cursor++;//按键值+1
 if (Cursor>4) Cursor=0;
 }
 Key0=millis();
}


void setup() 
{
  Serial.begin(9600);
  
  pinMode(KPin , INPUT_PULLUP);
  pinMode(limitSwitch, INPUT_PULLUP);
  pinMode(enablePin , OUTPUT);// 使能引脚为输出模式
  digitalWrite(enablePin , HIGH); //初始电机不进入工作状态
  
  display.begin(SSD1306_SWITCHCAPVCC, 0x3C);// 初始化OLED并设置其IIC地址为 0x3D（有些为0x3C）
  display.clearDisplay();
  
  stepperX.setMaxSpeed(3000);
  stepperX.setSpeed(200);
  stepperA.setMaxSpeed(3000);
  stepperA.setSpeed(200);
  // Create instances for MultiStepper - Adding the 2 steppers to the StepperControl instance for multi control
  StepperControl.addStepper(stepperX);
  StepperControl.addStepper(stepperA);
  
  attachInterrupt (digitalPinToInterrupt(2),Switch,RISING); // SW connected to D2

  Home(); //回零点
 
  display.clearDisplay();  // 清除屏幕
  display.setTextColor(WHITE); // 设置字体颜色,白色可见
  display.setTextSize(1);  //设置字体大小
  display.drawRect(0,0,128,64,SSD1306_WHITE);//绘制矩形外框
  display.setCursor(3 , 16);
  display.print("Start Point:");
  display.setCursor(3 , 25);
  display.print("Start Angle:");
  display.setCursor(3 , 34);
  display.print(" End  Point:");
  display.setCursor(3 , 43);
  display.print(" End  Angle:");
  display.setCursor(3 , 52);
  display.print("Total  Time:");
  display.drawLine(3,24,124,24,SSD1306_WHITE);//光标白线
  display.display();
  delay(2000);
  digitalWrite(enablePin , LOW);  // 将使能控制引脚设置为低电平从而让
  // 电机驱动板进入工作状态
  delay(2000);
  
}

void Home()
{
  //stepperX.setMaxSpeed(3000);
  //stepperX.setSpeed(200);
  //stepperA.setMaxSpeed(3000);
  //stepperA.setSpeed(200);
  if(digitalRead(limitSwitch)==1)//限位开关未被按下
  {  
    display.setTextColor(WHITE); // 设置字体颜色,白色可见
    display.setTextSize(10);  //设置字体大小
    display.setCursor(10, 10);
    display.print("Homing");
    display.display(); 
    }

  while (digitalRead(limitSwitch)==1) //限位开关未被按下，向限位开关负向运动
  {
    stepperX.setSpeed(-3000);
    stepperX.runSpeed();
    }
  delay(20);
  stepperX.setCurrentPosition(0);//复位步进电机初始位置
  stepperX.moveTo(200);//离开限位开关200
  while(stepperX.distanceToGo() != 0)
  {
    stepperX.setSpeed(3000);
    stepperX.runSpeed();
    }
    stepperX.setCurrentPosition(0);//离开限位开关200复位为步进电机初始位置
}

void SetTime()
{
 Xval = analogRead(XPIN);//读取摇杆X轴方向引脚A0的值，储存到摇杆X轴值中
 Yval = analogRead(YPIN);//读取摇杆Y轴方向引脚A1的值，储存到摇杆Y轴值中
while( Cursor==4)
{
  if(Xval>542.0){
    setTime = setTime+(Xval - 542.0) * 10/(1023.0-542.0);
    } else if(Xval<482.0){
     setTime = setTime-(Xval - 482.0) * 10 / 482.0;
     if(setTime<0) setTime=0;
    } 
  display.setTextSize(2);
  display.setTextColor(WHITE);
  display.setCursor(30,0);
  display.print("Speed");
  motorspeed=setspeed/80;
  display.setCursor(5,16);
  display.print(motorspeed);
  display.print(" mm/s");   
  totaldistance=XOutPoint-XInPoint;
  if(totaldistance<0)
  {
  totaldistance=totaldistance*(-1);
  }
  else
  {
              
  }
          timeinsec=(totaldistance/setspeed);
          timeinmins=timeinsec/60;
          display.setCursor(35,32);
          display.print("Time");
          display.setCursor(8,48);
          if(timeinmins>1)
          {
          display.print(timeinmins);
          display.print(" min");
          }
          else
          { 
          display.print(timeinsec);
          display.print(" sec");
          }
          display.display();
  }
          display.clearDisplay();
          display.setTextSize(2);
          display.setTextColor(WHITE);
          display.setCursor(30,0);
          display.print("Speed");
          motorspeed=setspeed/80;
          display.setCursor(5,16);
          display.print(motorspeed);
          display.print(" mm/s");   
          totaldistance=XOutPoint-XInPoint;
           if(totaldistance<0)
            {
              totaldistance=totaldistance*(-1);
            }
            else
            {
              
            }
          timeinsec=(totaldistance/setspeed);
          timeinmins=timeinsec/60;
          display.setCursor(35,32);
          display.print("Time");
          display.setCursor(8,48);
          if(timeinmins>1)
          {
          display.print(timeinmins);
          display.print(" min");
          }
          else
          { 
          display.print(timeinsec);
          display.print(" sec");
          }
          display.display();
  }
 
}

void stepperposition(int n)
{
  stepper1.setMaxSpeed(3000);
  stepper1.setSpeed(200);
  stepper2.setMaxSpeed(3000);
  stepper2.setSpeed(200);
  if (TurnDetected)  
  {
        TurnDetected = false;  // do NOT repeat IF loop until new rotation detected
     if(n==1)
     {
        if (!rotationdirection) 
        { 
          if( stepper1.currentPosition()-500>0 )
          {
          stepper1.move(-500);
          while(stepper1.distanceToGo() != 0)
            {
              stepper1.setSpeed(-3000);
              stepper1.runSpeed();
            }
          }
            else
            {
                while (stepper1.currentPosition()!=0) 
                 {
                      stepper1.setSpeed(-3000);
                      stepper1.runSpeed();
                 }
            }
        }

        if (rotationdirection) 
        { 
          if( stepper1.currentPosition()+500<61000 )
          {
          stepper1.move(500);
          while(stepper1.distanceToGo() != 0)
            {
              stepper1.setSpeed(3000);
              stepper1.runSpeed();
            }
          }
          else
          {
            while (stepper1.currentPosition()!= 61000) 
             {
                  stepper1.setSpeed(3000);
                  stepper1.runSpeed();
    
             } 
          }
        }
     }
     if(n==2)
     {
       if (rotationdirection) 
       { 
         stepper2.move(-100);
         while(stepper2.distanceToGo() != 0)
           {
             stepper2.setSpeed(-3000);
             stepper2.runSpeed();
           }      
       }
        if (!rotationdirection) 
       { 
         stepper2.move(100);
         while(stepper2.distanceToGo() != 0)
           {
             stepper2.setSpeed(3000);
             stepper2.runSpeed();
           } 
       }
     }
     }
} 


void loop()
{ 
  //Begin Setup
  if(flag==0)
  {
    display.clearDisplay();
    display.drawBitmap(0, 0, BeginSetup, 128, 64, 1);
    display.display(); 
    setspeed=200;
  }
  
  //SetXin
  if(flag==1)
  {
    display.clearDisplay();
    display.setTextSize(2);
    display.setTextColor(WHITE);
    display.setCursor(10,28);
    display.println("Set X In");
    display.display(); 
    while(flag==1)
    {
    stepperposition(1); 
    }
    XInPoint=stepper1.currentPosition();
  }
  //SetYin
  if(flag==2)
  {
    display.clearDisplay();
    display.setTextSize(2);
    display.setTextColor(WHITE);
    display.setCursor(10,28);
    display.println("Set Y In");
    display.display(); 
    while(flag==2)
    {
    stepperposition(2);
    }
    stepper2.setCurrentPosition(0);
    YInPoint=stepper2.currentPosition();
  }
  //SetXout
  if(flag==3)
  {
    display.clearDisplay();
    display.setTextSize(2);
    display.setTextColor(WHITE);
    display.setCursor(10,28);
    display.println("Set X Out");
    display.display(); 
    while(flag==3)
    {
    stepperposition(1);
    Serial.println(stepper1.currentPosition());
    }
    XOutPoint=stepper1.currentPosition();
    
  }
  //SetYout
  if(flag==4)
  {
    display.clearDisplay();
    display.setTextSize(2);
    display.setTextColor(WHITE);
    display.setCursor(10,28);
    display.println("Set Y Out");
    display.display();
    while(flag==4)
    {
    stepperposition(2);
    }
    YOutPoint=stepper2.currentPosition();
    display.clearDisplay();
        
    // Go to IN position
    gotoposition[0]=XInPoint;
    gotoposition[1]=YInPoint;    
    display.clearDisplay();
    display.setCursor(8,28);
    display.println(" Preview  ");
    display.display(); 
    stepper1.setMaxSpeed(3000);
    StepperControl.moveTo(gotoposition);
    StepperControl.runSpeedToPosition();
  }

  //Display Set Speed
  if(flag==5)
  {
    display.clearDisplay();
    display.setCursor(8,28);
    display.println("Set Speed");
    display.display();  
  }
  //Change Speed
  if(flag==6)
  {
    display.clearDisplay();
    SetSpeed();
  }
  //DisplayStart
  if(flag==7)
  {
    display.clearDisplay();
    display.setCursor(30,27);
    display.println("Start");
    display.display();
  }
  //Start
  if(flag==8)
  { 
    display.clearDisplay();
    display.setCursor(20,27);
    display.println("Running");
    display.display();
    Serial.println(XInPoint);
    Serial.println(XOutPoint);
    Serial.println(YInPoint);
    Serial.println(YOutPoint);

    gotoposition[0]=XOutPoint;
    gotoposition[1]=YOutPoint;
    stepper1.setMaxSpeed(setspeed);
    StepperControl.moveTo(gotoposition);
    StepperControl.runSpeedToPosition();
    flag=flag+1;
  }
  //Slide Finish
   if(flag==9)
   {
    display.clearDisplay();
    display.setCursor(24,26);
    display.println("Finish");
    display.display();
   }
  //Return to start
   if(flag==10)
   {
    display.clearDisplay();
    Home();
    flag=0;
   }  
}