libraries/PololuMotorController/pmc.cpp
// Public: Set a speed (0 to 127) and direction (boolean) to a motor
Set a speed (0 to 127) and direction (boolean) to a motor
Set a speed ( 0 to 127 ) and direction ( boolean ) to a motor
void MotorController::SetMotor(int motorIndex, int speed, boolean GoForward)
{
	unsigned char buffer[4];
	buffer[0] = 0x80;
	buffer[1] = 0x00;
	unsigned char index = 0;
	if(motorIndex > 0 || motorIndex < 0)
		index = 2;
	if(GoForward)
		buffer[2] = 0x00 | index;
	else
		buffer[2] = 0x01 | index;
	unsigned char targetSpeed = 0;
	if(speed >= 128)
		targetSpeed = 127;
	else if(speed < 0)
		targetSpeed = 0;
	else
		targetSpeed = (unsigned char)speed;
	buffer[3] = targetSpeed;
	for(int i = 0; i < 4; i++)
	{
	  digitalWrite(tx,LOW);
      delayMicroseconds(BITDELAY);
      for (byte mask = 0x01; mask>0; mask <<= 1) 
      { 
        digitalWrite(tx, buffer[i] & mask ? HIGH : LOW);
        delayMicroseconds(BITDELAY);
      }
      digitalWrite(tx, HIGH);
      delayMicroseconds(BITDELAY);
	}
	delay(10);
}
void MotorController :: SetMotor ( int motorIndex , int speed , boolean GoForward ) { unsigned char buffer [ 4 ] ; buffer [ 0 ] = 0x80 ; buffer [ 1 ] = 0x00 ; unsigned char index = 0 ; if ( motorIndex > 0 || motorIndex < 0 ) index = 2 ; if ( GoForward ) buffer [ 2 ] = 0x00 | index ; else buffer [ 2 ] = 0x01 | index ; unsigned char targetSpeed = 0 ; if ( speed >= 128 ) targetSpeed = 127 ; else if ( speed < 0 ) targetSpeed = 0 ; else targetSpeed = ( unsigned char ) speed ; buffer [ 3 ] = targetSpeed ; for ( int i = 0 ; i < 4 ; i ++ ) { digitalWrite ( tx , LOW ) ; delayMicroseconds ( BITDELAY ) ; for ( byte mask = 0x01 ; mask > 0 ; mask <<= 1 ) { digitalWrite ( tx , buffer [ i ] & mask ? HIGH : LOW ) ; delayMicroseconds ( BITDELAY ) ; } digitalWrite ( tx , HIGH ) ; delayMicroseconds ( BITDELAY ) ; } delay ( 10 ) ; }
Public: Set a speed (0 to 127) and direction (boolean) to a motor
Public : Set a speed ( 0 to 127 ) and direction ( boolean ) to a motor
// Create the packet buffer // Start byte and device ID byte // Set motor index // Motor number and direction // Motor speed // Send the packet	