Esempio n. 1
0
	task usercontrol()
	{
		// User control code here, inside the loop

		while (true)
		{
			motor[DFR]=vexRT[Ch2];
			motor[DBR]=vexRT[Ch2];
			motor[DFL]=vexRT[Ch2];
			motor[DBL]=vexRT[Ch2];
			motor[middle]=vexRT[Ch1];

			if(abs(vexRT[Ch4]) > MIN_JOYSTICK_ACTIVATION_THRESHOLD)
			{
				turnRight(127);
			}
			else
			{
				turnRight(0);
			}
				if(abs(vexRT[Ch3]) > tol)//set straight
			straight = vexRT[Ch3];
		else
			straight = 0;
		if(abs(vexRT[Ch1]) > tol) //set ptturn
			ptturn = vexRT[Ch1];
		else
			ptturn = 0;
		if(abs(vexRT[Ch4]) > tol) //set side
			side = vexRT[Ch4];
		else
			side = 0;
		if((abs(-ptturn + straight)<=127) && (abs(ptturn + straight)<=127))
		{
			driveRight(-ptturn/2 + straight*2/3);
			driveLeft(ptturn/2 + straight*2/3);
			motor[holo]= side;
		}
		else if(abs(-ptturn + straight) > 127)//(pttrn + straight)
		{
			driveRight(100);
			driveLeft(-100);
			motor[holo]= side;
		}
		else if(abs(-ptturn + straight) > 127)
		{
			driveRight(-100);
			driveLeft(100);
			motor[holo]= side;
		}
		}
	}
void *displayStart()
{

	slotA2D();
	slotI2C();
	setDisplayToOutput();
	
	i2cFileDesc = initI2cBus(I2CDRV_LINUX_BUS2, I2C_DEVICE_ADDRESS);

	writeI2cReg(i2cFileDesc, REG_DIRA, 0x00);
	writeI2cReg(i2cFileDesc, REG_DIRB, 0x00);

	//Clear Display
	writeI2cReg(i2cFileDesc, REG_BOT, 0x00);
	writeI2cReg(i2cFileDesc, REG_TOP, 0x00);
	


	deActivateRight();
	deActivateLeft();
	while(1) {
	//Do stuff here
		driveLeft();
		driveRight();
	}
	deActivateRight();
	deActivateLeft();
	close(i2cFileDesc);
}
void drive_straight(float dist, int speed)
{
	while((nMotorEncoder[DBR] < 360 * dist / (4 * PI) + error) || (nMotorEncoder[DBR] < 360 * dist / (4 * PI) - error))
	{
		driveRight(speed);
		driveLeft(speed);
	}
}
Esempio n. 4
0
// ----------------------------------------------------------------------------
//  drive using light ambient sensors for directional command
// ------------------------------------------------------------[ driveByLight ]
void WRTbotDrive::driveByLight( wrt_motor_t &motor )
{
	// -------------------------------------------------------------------
	if( ( 0 == (motor.left.control  & wrt_motorControl_power_bit) ) ||
		( 0 == (motor.right.control & wrt_motorControl_power_bit) ) )
	{
		WRTbotDrive::stop();

		return;
	}

	// --- read ambient light sensor
	// -------------------------------------------------------------------
	sensor.light.data = analogRead( wrt_pin_ambientLight_sensor );

	Serial.println(" ");
	Serial.print( sensor.light.data );	Serial.print( " , " );

    // --- convert to ambient light units
	// -------------------------------------------------------------------
	sensor.light.data *= sensor.light.factor;

	Serial.print( sensor.light.data );	Serial.println( " , " );


	useNominal = true;
	// -------------------------------------------------------------------
	if( (sensor.light.data > sensor.light.limitMin) && 			// minimum limit
		(sensor.light.data < sensor.light.limitLowerBound) )	// highest lower bound
	{

		Serial.print( "driveLeft" );	Serial.println( "   " );


		driveLeft( motor );
	}
	else if( (sensor.light.data > sensor.light.limitUpperBound) &&	// lowest  upper bound
			 (sensor.light.data < sensor.light.limitMax) )			// maximum limit
	{

		Serial.print( "driveRight" );	Serial.println( "   " );

		driveRight( motor );
	}
	else
	{
		Serial.print( "driveForward" );	Serial.println( "  now " );

		driveForward( motor );
	}

}	// done driveByLight()
Esempio n. 5
0
task main(){

  motor[motorB] = 0;
  motor[motorC] = 0;
  drivefor(1,75);
  driveRight(75);
  driveLeft(75);
  drivefor(1,-75);
  encodeTurnB(360, 25);
  encodeTurnC(360, 25);
  simpleTurn("B",3);
  simpleTurn("C",3);
}
Esempio n. 6
0
void Motor_run()
{
  if(MPI_check_available(MOTOR_DRIVER_PID) == true)
    {
      MPI_get_message(MOTOR_DRIVER_PID, &incomingPID, &cmdSize, incomingMsg);

      if(cmdSize == sizeof(int))
	{
	  OS_memcpy(&incomingCmd, incomingMsg, cmdSize);

	  switch(incomingCmd)
	    {
	    case C_DRIVE_FORWARD:
	      UART_send("drive forward\r\n", CONSOLE_BASE);
	      driveForward();

	      break;

	    case C_DRIVE_REVERSE:
	      UART_send("drive reverse\r\n", CONSOLE_BASE);
	      driveReverse();

	      break;

	    case C_DRIVE_LEFT:
	      UART_send("drive left\r\n", CONSOLE_BASE);
	      driveLeft();
	      
	      break;

	    case C_DRIVE_RIGHT:
	      UART_send("drive right\r\n", CONSOLE_BASE);
	      driveRight();

	      break;

	    case C_DRIVE_HALT:
	      UART_send("drive halt\r\n", CONSOLE_BASE);
	      driveHalt();

	      break;

	    }

	}

    }

}
void turn(int a)
{
	driveRight(-a);
	driveLeft(a);
}
Esempio n. 8
0
//  drive the robot
// -------------------------------------------------------------------[ drive ]
void WRTbotDrive::drive( const uint16_t driveThisWay, wrt_motor_t &motor )
{
	// --- motor ON by default
	// -----------------------
	motor.left.control  |= wrt_motorControl_power_ON;
	motor.right.control |= wrt_motorControl_power_ON;

	// --- no drive by light
	// --------------------------
	this->driveByLightFactor = 0;

	switch( driveThisWay )
	{	default:
		// ------------------------------------------------------------[ stop ]
		case wrt_stop:
		{
			motor.left.control  &= wrt_motorControl_power_OFF;
			motor.right.control &= wrt_motorControl_power_OFF;
			// ---
			motor.left.speed   = 0;
			motor.right.speed  = 0;
			// ---
			drive( motor );

		}	break;
		// ---------------------------------------------------------[ forward ]
		case wrt_forward:
		{
			this->useNominal = true;
			// ---
			driveForward( motor );

		}	break;
		// ---------------------------------------------------------[ forward ]
		case wrt_forward_slow:
		{
			this->useNominal = false;
			motor.right.speed = wrt_code_motor_speed_min;
			motor.left.speed  = wrt_code_motor_speed_min;
			// ---
			driveForward( motor );

		}	break;
		// ----------------------------------------------------[ fast forward ]
		case wrt_forward_fast:
		{
			this->useNominal = false;
			motor.right.speed = wrt_code_motor_speed_max;
			motor.left.speed  = wrt_code_motor_speed_max;
			// ---
			driveForward( motor );

		}	break;
		// ---------------------------------------------------------[ reverse ]
		case wrt_reverse:
		{
			this->useNominal = true;
			// ---
			driveReverse( motor );

		}	break;
		// ----------------------------------------------------[ fast reverse ]
		case wrt_reverse_fast:
		{
			this->useNominal = false;
			motor.right.speed = wrt_code_motor_speed_max;
			motor.left.speed  = wrt_code_motor_speed_max;
			// ---
			driveReverse( motor );

		}	break;
		// -------------------------------------------------------[ left turn ]
		case wrt_drive_left:
		{
			driveLeft( motor );

		}	break;
		// --------------------------------------------------[ fast left turn ]
		case wrt_turn_left_fast:
		{
			turnLeftFast( motor );

		}	break;
		// --------------------------------------------------[ slow left turn ]
		case wrt_turn_left:
		{
			this->useNominal = true;

			turnLeft( motor );

		}	break;
		// ------------------------------------------------------[ right turn ]
		case wrt_drive_right:
		{
			driveRight( motor );

		}	break;
		// -------------------------------------------------[ fast right turn ]
		case wrt_turn_right_fast:
		{
			turnRightFast( motor );

		}	break;
		// -------------------------------------------------[ slow right turn ]
		case wrt_turn_right:
		{
			this->useNominal = true;

			turnRight( motor );

		}	break;
		// -----------------------------------------[ differential right turn ]
		case wrt_turnRightDiff:
		case wrt_turnRightDiff_fast:
		{
			turnRightDiferential( driveThisWay, motor );

		}	break;
		// -----------------------------------------[ differential left turn ]
		case wrt_turnLeftDiff:
		case wrt_turnLeftDiff_fast:
		{
			turnLeftDiferential( driveThisWay, motor );

		}	break;
	}


}	// done drive()