void motor1SetSpeed(int speed)
{
speed = limitSpeedMax(speed);
//if ((speed <2) && (speed> -2)) motor1Forward(0); // Break by putting both H-Bridge sides High
if (speed >= 0) motor1Forward(speed);
if (speed < 0) motor1Reverse(-speed);
}
void Key_Scan1(void)
{
switch(Key_Scan_Steps1)
{
case 0:
if(keyMotor1Forward == 0 || keyMotor1Backward == 0 || keyMotor2Copy == 0)
{
Key_Scan_Steps1 = 1;
}
break;
case 1:
if(keyMotor1Forward == 0 || keyMotor1Backward == 0 || keyMotor2Copy == 0)
{
Key_Scan_Steps1 = 2;
}
else
{
Key_Scan_Steps1 = 0;
}
break;
case 2:
if(keyMotor1Forward == 0)
{
motor1Forward();
}
else if(keyMotor1Backward == 0)
{
motor1Backward();
}
else if(keyMotor2Copy == 0)
{
motor2Copy();
}
Key_Scan_Steps1 = 3;
break;
case 3:
if(keyMotor1Forward == 1 && keyMotor1Backward == 1 && keyMotor2Copy == 1)
{
Key_Scan_Steps1 = 0;
}
break;
default:
_nop_();
}
}
void CompactQik2s9v1::stopMotor1()
{
motor1Forward(0);
}
int main(void)
{
pinIO();
serialSetupUSB();
serialSetupP2P();
timerSetup();
//pwmSetup();
setupA2D();
motorSetup();
encSetup();
int motor1;
//int motor1speed = 200;
int motor2speed = 200;
char targetVelocity1 = 0;
char currentVelocity;
float pop;
char error = 0;
//infinete loop in which we count
while(1)
{
//currentVelocity = velocityGet();
motor1 = motorSet(39);
motor1 = 200;
//targetVelocity1 = 30;
//error = targetVelocity1 - currentVelocity;
//int test
/* if (currentVelocity > targetVelocity1)
{
if(motor1speed > 0)
{
motor1speed -=2;
} else {
motor1speed = 0;
}
} else if (currentVelocity < targetVelocity1)
{
if(motor1speed < 798)
{
motor1speed +=2;
} else {
motor1speed = 798;
}
//motor1speed = 798*(float)(1-(float)(currentVelocity / targetVelocity1));
}*/
motor1Forward(motor1);
motor2Forward(motor2speed);
} //remain here forever, never end the main function.
return 0; //we should never really return
}
