118 lines
2.6 KiB
C++
118 lines
2.6 KiB
C++
// 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
|
|
}
|