void max7221::put( byte reg, byte data )
{
/*
* Updates one register of our chip by first selecting the register byte reg
* and then putting the data byte data and switching the LOAD pin once.
*/
latch(0);
putByte( reg );
putByte( data );
noOp(_chipselect);
latch(1);
}
void Distkeep()
{
int DistFrom = 30; // in Cm
int Deadband = 2;
int Distmin = 35;
// In Inches
float COUNTS_PER_INCH = 90.55;
nMotorEncoder[R_Motor] = 0;
nMotorEncoder[L_Motor] = 0;
servo[IRS_1] = 160;
servo[Block_Chuck] = 000;
ClearTimer(T1);
while (SensorValue[IR] != 6 && nMotorEncoder[ L_Motor] / COUNTS_PER_INCH < Distmin && nMotorEncoder[L_Motor] / COUNTS_PER_INCH < Distmin && time100[T1] < 50 )
{
if ((USreadDist(SONAR_1) < DistFrom - Deadband) && (nMotorEncoder[ L_Motor] / COUNTS_PER_INCH > 18))
{
motor[L_Motor] = 25;
motor[R_Motor] = 50;
}
else if ((USreadDist(SONAR_1) > DistFrom + Deadband) && (nMotorEncoder[ L_Motor] / COUNTS_PER_INCH > 18))
{
motor[L_Motor] = 50;
motor[R_Motor] = 25;
}
else
{
motor[L_Motor] = 50;
motor[R_Motor] = 50;
}
if (time100[T1] > 50 || nMotorEncoder[L_Motor] / COUNTS_PER_INCH > 40)
{
motor[L_Motor] = 0;
motor[R_Motor] = 0;
powerOff();
noOp();
alive();
StopAllTasks();
wait10Msec(55);
}
}
