motorwack

arduino_maze/motorControl.ino

@@ -0,0 +1,118 @@
+// 3.6 2 amps
+
+
+const int rangeSTick = 1023; // 0 is bottom
+const int reangle = 45, angleRange = 90; // 0 is bottom
+const float transformDesireToAngle = .09; // tranpoe
+const int stillJoystick = 511;
+const int deadband = 10;
+const int motorPulseDelay = 500;
+
+const float angleChange = 1.8;
+float x = 0, y = 0; //angle of current postion
+ 
+class MotorControl
+{
+  public:
+  float currentPostion;
+  float goPostion;
+  int pastDesire = stillJoystick;
+  bool reset;
+  bool isButtonPressed;
+  int button;
+  int direct, pulse;
+  
+    MotorControl(int dire, int puls)
+    {
+      pinMode(dire,OUTPUT);
+      pinMode(puls,OUTPUT);
+      direct = dire;
+      pulse = puls;
+      currentPostion = reangle;
+    }
+    void whatToDo(int desire)
+    {
+      if (desire < 0)
+      button = desire * -1;
+      else
+      goto yeet;
+      switch (button)
+      {
+        default:
+        goto yeet;
+        case 1:
+goto caseEnd;
+        case 2:
+goto caseEnd;
+        case 3:
+goto caseEnd;
+        case 4:
+goto caseEnd;
+        case 5:
+goto caseEnd;
+        case 6:
+goto caseEnd;
+        case 7: 
+      caseEnd:  
+      button = 0;
+      return; 
+      }
+      yeet:
+      if(desire > (pastDesire + 10) || desire < (pastDesire - 10)) // noise from constant stick upload
+      {
+        goPostion = tranpose(desire);
+        pastDesire = desire;
+      }
+
+      // two pins
+      if(goPostion > (currentPostion + 5.0))
+      {
+        digitalWrite(direct, HIGH);
+        currentPostion += angleChange;
+        digitalWrite(pulse,HIGH); 
+        delayMicroseconds(motorPulseDelay); 
+        digitalWrite(pulse,LOW); 
+        delayMicroseconds(motorPulseDelay);
+      }
+      return;
+      if((currentPostion - 5.0) > goPostion)
+      {
+        digitalWrite(direct, LOW);
+        currentPostion -= angleChange;
+        digitalWrite(pulse,HIGH); 
+        delayMicroseconds(motorPulseDelay); 
+        digitalWrite(pulse,LOW); 
+        delayMicroseconds(motorPulseDelay);
+      }
+    }
+    float tranpose(int desire)
+    {
+      int temp = desire * transformDesireToAngle;
+      return temp;
+    }
+    void calibrate()
+    {
+      currentPostion = reangle;
+      goPostion = reangle;
+    }
+};
+
+
+// defines pins numbers
+const int stepPinM1 = 3; // motor 1
+const int dirPinM1 = 5;
+const int stepPinM2  = 6; // motor 2
+const int dirPinM2 = 9;
+
+MotorControl xMotor(stepPinM1, dirPinM1);
+MotorControl yMotor(stepPinM2, dirPinM2);
+
+void setup() {
+  
+}
+
+
+void loop() {
+  xMotor.whatToDo(700);// angle
+  yMotor.whatToDo(700);// angle
+}