Motor Control

using System;
using System.Timers;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading;
using System.Diagnostics;
using System.Windows;
using System.Windows.Controls;
using System.Windows.Data;
using System.Windows.Documents;
using System.Windows.Input;
using System.Windows.Media;
using System.Windows.Media.Imaging;
using System.Windows.Navigation;
using System.Windows.Shapes;


using WpfCalculator;

namespace RotaryTable
{
    /// <summary>
    /// Interaction logic for MainWindow.xaml
    /// </summary>
    public partial class MainWindow : Window
    {
        public MainWindow()
        {
            InitializeComponent();
        }

        StepperWindow stepper = null;
        StepperWindow stepper2 = null;
        Calculator c = null;
        Stopwatch time = new Stopwatch();
        Stopwatch testt = new Stopwatch();



        int Mode = 0;
        int State = 0;

        private void Window_Loaded(object sender, RoutedEventArgs e)
        {
            stepper = new StepperWindow(304413, "Top Motor");
            //stepper = new StepperWindow(269156, "Rotary Table - Rev6");

            stepper.SetScale(3000);
            stepper.SetAltIncrement(360);
            stepper.CurrentLimit = 2.0;
            stepper.Show();
            stepper.SetDial(0.0);
            stepper.StopStartActive = true;
            stepper.PositionChanged += new PhidgetPositionEventHandler(getPosn);

            stepper2 = new StepperWindow(268801, "Bottom Motor");
            //stepper2 = new StepperWindow(268801, "X-drive (Rotary Table - Rev6)");

            stepper2.SetScale(3000);
            stepper2.SetAltIncrement(360);
            stepper2.CurrentLimit = 2.0;
            stepper2.Show();
            stepper2.SetDial(0.0);
            stepper2.StopStartActive = false;
            stepper2.PositionChanged += new PhidgetPositionEventHandler(getPosn2);
        }

        private void Window_Unloaded(object sender, RoutedEventArgs e)
        {
            stepper.engagedCheckBox.IsChecked = false;
            stepper2.engagedCheckBox.IsChecked = false;
            if (stepper != null) stepper.Close();
            if (stepper2 != null) stepper2.Close();
            if (c != null) c.Close();
        }


        double ratio = 1.5; //Ratio from Stepper motor to park interlock if different ratios were used.
        double acceleration = .1; //Hard coded.
        double speed = 0; //Initialized by user.

        double angle = 0;
        double currPos = 0;
        double currPos2 = 0; //Divides GetPosn by 20 for stepper2 to be equal to stepper

        double angleConv = -360 / 1.0667; //Converts position to angle

        double currAngle = 0;

        int count = 0;

        long t = 0;

        long time0 = 1;
        long time1 = 0;
        long time2 = 0;

        int prevAngle = -1;

        bool direction = false;//True if bottom motor is going slower, false if going faster.
        bool move = false;
        bool start = false;
        bool find = false;  //Set true if angles want to be found.

        bool check0 = false;
        bool check150 = false;
        bool check300 = false;

        double angle0=10; //Set all values to 1000 if find is set to true.
        double angle150=190;
        double angle300=380;

        //Sequence currently only goes from 0 to 150 to 300 and both 150 and 300 must have times set
        public void getPosn(object sender, PhidgetPositionEventArgs e)
        {
            if (Mode != 3) return;

            //if (!e.m_complete) return;

            //UI update for switch conditions
            Input0.IsChecked = stepper.input0Chk.IsChecked;
            Input1.IsChecked = stepper.input1Chk.IsChecked;
            Input2.IsChecked = stepper.input2Chk.IsChecked;
            Input3.IsChecked = stepper.input3Chk.IsChecked;


            //Angle updater
            currPos = e.m_position;
            //currAngle = (currPos2 + currPos - 90000) * angleConv;
            //CurrAngle.Text = currAngle.ToString();
            t = time.ElapsedMilliseconds;

            //Moves the cam to the initial position.
            if(start==true&&(stepper.input0Chk.IsChecked==false&&stepper.input1Chk.IsChecked==true))
            {
                start=false;
                stepper.SetVelocityRpm(0);
                stepper.IsEnabled=false;
                Initialize();
            }

            //Condition for angle 0 of cam
            if (prevAngle == 0 && t >= time0)
            {
                check0 = false;
                time.Reset();
                prevAngle = 1;
                direction = false;
                stepper2.SetVelocityRpm(speed * 1.5 * ratio);
                if (find == false)
                {
                    angle = angle150;
                    move = true;
                }

            }
            //Condition for angle 150 of cam
            else if (prevAngle == 1 && t >= time1)
            {
                check150 = false;
                time.Reset();
                prevAngle = 2;
                stepper2.SetVelocityRpm(speed * 1.5 * ratio);
                direction = false;
                if (find == false)
                {
                    angle = angle300;
                    move = true;
                }
            }
            //Condition for angle 300 of cam
            else if (prevAngle == 2 && t >= time2)
            {
                check300 = false;
                time.Reset();
                prevAngle = 0;
                direction = true;
                stepper2.SetVelocityRpm(speed * .5 * ratio);
                if (find == false)
                {
                    angle = angle0;
                    move = true;
                }
            }


            //Checks for mechanical error after a 100ms delay
            if (check0 == true&&t>100)
            {
                if (stepper.input0Chk.IsChecked == true || stepper.input1Chk.IsChecked == false)
                {
                    go.Content = "ERROR 01";
                    Stop_Click(null, null);
                }
            }
            if (check150 == true&&t>100)
            {
                if (stepper.input2Chk.IsChecked == false || stepper.input3Chk.IsChecked == false)
                {
                    go.Content = "ERROR 23";
                    Stop_Click(null, null);
                }
            }
            if (check300 == true&&t>100)
            {
                if (stepper.input3Chk.IsChecked == false)
                {
                    go.Content = "ERROR 3";
                    Stop_Click(null, null);
                }
            }


        }

        public void getPosn2(object sender, PhidgetPositionEventArgs e)
        {
            if (Mode != 3) return;


            //Angle updater
            currPos2 = e.m_position;
            currAngle = (currPos2 + currPos - 90000) * angleConv;
            CurrAngle.Text = currAngle.ToString();

            //Sets initial angle positions.
            if (find == true)
            {
                if (angle150 == 1000 && stepper.input2Chk.IsChecked == true && stepper.input3Chk.IsChecked == true)
                {
                    stepper2.SetVelocityRpm(speed * ratio);
                    angle150 = currAngle;
                    time.Start();
                }
                if (angle300 == 1000 && stepper.input3Chk.IsChecked == true)
                {
                    stepper2.SetVelocityRpm(speed * ratio);
                    angle300 = currAngle;
                    time.Start();
                }
                if (angle0 == 10000 && stepper.input0Chk.IsChecked == false && stepper.input1Chk.IsChecked == true)
                {
                    stepper2.SetVelocityRpm(speed * ratio);
                    angle0 = currAngle;
                    find = false;
                    time.Start();
                }
            }



            //Returns motor to original speed once it reaches the angle specified.
            if (currAngle>=angle && direction == false&&move==true)
            {
                move = false;
                time.Start();
                stepper2.SetVelocityRpm(speed * ratio);
                if (angle < 300)
                {
                    check150 = true;
                }
                if (angle > 300)
                {
                    check300 = true;
                }
            }
            if (currAngle <= angle && direction == true&&move==true)
            {
                move = false;
                count++;
                Count.Text = count.ToString();
                time.Start();
                stepper2.SetVelocityRpm(speed * ratio);
                check0 = true;
            }

        }


        private void Button_Click(object sender, RoutedEventArgs e)
        {
            //Initializes speed and position
            speed = Convert.ToSingle(Speed.Text);
            if (speed < 0 || speed > 1000)
            {
                Speed.Text = "0";
                speed = 0;
            }
            speed = speed / 18000 * 53.6;

            acceleration = Convert.ToDouble(Acceleration.Text);
            stepper.SetAcceleration(acceleration * 56);
            stepper2.SetAcceleration(acceleration * 56);
            if (stepper.input0Chk.IsChecked == true || stepper.input1Chk.IsChecked == false)
            {
                stepper.SetVelocityRpm(speed * ratio*.1);
                start = true;
                stepper.SetPosn(90001);
                stepper.engagedCheckBox.IsChecked = true;
            }
            else
            {
                Initialize();
            }


        }



        private void Initialize()
        {
            //Variables
            stepper.SetVelocityRpm(speed * ratio);
            stepper2.SetVelocityRpm(speed * ratio);
            stepper.SetDial(45000);
            stepper2.SetDial(45000);
            stepper.SetPosn(90000);
            stepper2.SetPosn(0);

            if (Angle0.Text != "")
            {
                angle0 = Convert.ToDouble(Angle0.Text);
            }
            if (Angle150.Text != "")
            {
                angle150 = Convert.ToDouble(Angle150.Text);
            }
            if (Angle300.Text != "")
            {
                angle300 = Convert.ToDouble(Angle300.Text);
            }


            //initializes conditions for angle sequence
            time0 = Convert.ToInt32(zero.Text);
            if (time0 <= 0)
            {
                time0 = 1;
            }
            time0 = time0 * 1000;
            time1 = Convert.ToInt32(onefifty.Text) * 1000;
            time2 = Convert.ToInt32(three.Text) * 1000;

            if (time1 > 0 || time2 > 0)
            {
                time.Start();
                prevAngle = 0;

            }

            //Starts operation
            Start.IsEnabled = false;
            stepper.engagedCheckBox.IsChecked = true;
            stepper2.engagedCheckBox.IsChecked = true;
            Stop.IsEnabled = true;
            testt.Start();
            check0 = true;
        }



        //Stops the test operation
        private void Stop_Click(object sender, RoutedEventArgs e)
        {
            TestTime.Text = testt.Elapsed.ToString();
            count = 0;
            time.Reset();
            testt.Reset();
            check0 = false;
            check150 = false;
            check300 = false;
            stepper.SetVelocityRpm(0);
            stepper.engagedCheckBox.IsChecked = false;
            stepper2.SetVelocityRpm(0);
            stepper2.engagedCheckBox.IsChecked = false;
            stepper.SetDial(stepper.GetCurrentPosition());
            stepper2.SetDial(stepper2.GetCurrentPosition());
            Stop.IsEnabled = false;
            Start.IsEnabled = true;
        }

    }
}