void CPMotion::ZigZag(short Velocity, short Delay, short Iterations)
{
//OK
m_Robot->SetMaxSpeed(360);
double OriginalHeading = m_Robot->GetHeading();
double Angle1 = pi / 12;
double Angle2 = -pi / 6;
double Angle3 = pi / 6;
TurnToAngle(Velocity, OriginalHeading, Angle1);
SWait(1);
for (int i = 1; i < Iterations; i++)
{
MoveForwardNS(Velocity, 2);
TurnToAngle(Velocity, OriginalHeading, Angle2);
MoveForwardNS(Velocity, 2);
TurnToAngle(Velocity, OriginalHeading, Angle3);
}
MoveForward(Velocity, 1);
m_Robot->SetHeadingDestination(OriginalHeading, Velocity, 2);
SWait(Delay);
m_Robot->Stop();
}
void CPMotion::AbsurdMotion(short Ki, short Kd)
{
//OK
m_Robot->SetMaxSpeed(360);
//set unstable values:
//Ki = 100 'strong jitter !
//Kd = 80 'wildly erratic !
double OriginalHeading = m_Robot->GetHeading();
robPOINT StartPoint, Destination;
StartPoint = m_Robot->GetPosition();
Destination = StartPoint; //going nowhere
short Kp, Kb, Kpwm;
int Velocity = 300;
//default parameters for MotorControlConstants:
Kp = 3;
Kb = 8;
Kpwm = 0;
//Kd = 5 ; //disabled to create absurd motion
//Ki = 6;
m_Robot->SetMotorControlConstants(Kp, Ki, Kd, Kb, Kpwm);
m_Robot->SetMotorControlConstants(Kp, Ki, Kd, Kb, Kpwm);
m_Robot->AddPositionVelocityToInstructions(Destination, NULL, cLrgPositionZone, 300);
m_Robot->FollowInstructions(true, false);
SWait(5);
//reset:
m_Robot->Stop();
ResetMotorConstants();
}
void CPMotion::Smooth(short Velocity, short Duration)
{
//OK
RampUp(Velocity);
SWait(Duration);
RampDown(Velocity);
}
int main()
{int i,j=0;
i =2;
while(j<500)
{
SWait(5);
SWait(i);
SWait((i+1)%5);
SIncrement(i);
//Eat
SSignal(i);
SSignal((i+1)%5);
SSignal(5);
//Think
j++;
}
}
void CPMotion::Staccato(short Velocity, short Duration, short Iterations)
{
//do only 3 interations to demo
for (int i = 1; i < Iterations; i++)
{
RampUp(Velocity);
RampDown(Velocity);
SWait(Duration);
}
}
void CPMotion::Spinn(short Velocity, short Duration, bool Direction)
{
//overshoots on fast spinns; use correction...later...
m_Robot->SetMaxSpeed(360);
double limit;
double FoundHeading = m_Robot->GetHeading();
double Correction;
if (Velocity <= 100)
{
Correction = pi / 4;
limit = 0.1;
}
else if (Velocity = 200)
{
Correction = pi / 3;
limit = 0.3;
}
else if (Velocity > 200)
{
Correction = pi / 3;
limit = 0.4;
}
double NewHeading = FoundHeading - Correction;
if (Direction == 1)
{
m_Robot->SetVelocities(Velocity, -Velocity); //R
}
else
{
m_Robot->SetVelocities(-Velocity, Velocity); //L
}
SWait(Duration);
FoundHeading = m_Robot->GetHeading();
while (fabs(FoundHeading - NewHeading) > limit)
{
m_Robot->DoWindowMessages();
FoundHeading = m_Robot->GetHeading();
}
m_Robot->Stop();
}
void CPMotion::OneWheelSpinn(short Velocity, short Wheel, short Duration, bool Direction, bool Stop)
{
//OK for now
m_Robot->SetMaxSpeed(360);
double FoundHeading = m_Robot->GetHeading();
double NewHeading = FoundHeading;
if (Wheel == 1) //R
{
if (Direction == 1)
{
m_Robot->SetVelocities(Velocity, 0);
}
else
{
m_Robot->SetVelocities(-Velocity, 0);
}
}
else
{
if (Direction == 1)
{
m_Robot->SetVelocities(0, Velocity);
}
else
{
m_Robot->SetVelocities(0, -Velocity);
}
}
SWait(Duration);
FoundHeading = m_Robot->GetHeading();
while (fabs(FoundHeading - NewHeading) > 0.25) //was 0.2
{
m_Robot->DoWindowMessages();
FoundHeading = m_Robot->GetHeading();
}
if (Stop)
{
m_Robot->Stop();
}
}
void CPMotion::LeftRight(short Velocity, short Delay, short Iterations)
{
//OK
m_Robot->SetMaxSpeed(360);
double OriginalHeading = m_Robot->GetHeading();
for (int i = 1; i < Iterations; i++)
{
m_Robot->AddVelocitiesToInstructions(Velocity, 0, 1000);
m_Robot->AddVelocitiesToInstructions(0, Velocity, 1000);
}
m_Robot->FollowInstructions();
m_Robot->SetHeadingDestination(OriginalHeading, Velocity, 2);
SWait(Delay);
m_Robot->Stop();
}
void CPMotion::Spinn360()
{
//for testing purposes only
m_Robot->SetMaxSpeed(360);
double FoundHeading = m_Robot->GetHeading();
double NewHeading = FoundHeading;
m_Robot->SetVelocities(50, -50); //R
SWait(4);
FoundHeading = m_Robot->GetHeading();
while (fabs(FoundHeading - NewHeading) > 0.2)
{
m_Robot->DoWindowMessages();
FoundHeading = m_Robot->GetHeading();
}
m_Robot->Stop();
}
