C++ Drive类代码示例

	void TeleopPeriodic()
	{
		if(!isWait)
		{
			//drive
			drive->drive(oi->joyDrive->GetRawAxis(FORWARD_Y_AXIS)), setTurnSpeed(oi->joyDrive->GetRawAxis(TURN_X_AXIS));
			drive->shift(oi->joyDrive->GetRawButton(8), oi->joyDrive->GetRawButton(9));
			//manipulator
			manip->moveArm(oi->joyManip->GetRawButton(6), oi->joyManip->GetRawButton(7));
			manip->intakeBall(oi->joyManip->GetRawButton(INTAKE_BUTTON), oi->joyManip->GetRawButton(OUTTAKE_BUTTON), (oi->getManipJoystick()->GetThrottle()+1)/2);
			catapult->launchBall();
			manip->toggleCompressor(oi->joyDrive->GetRawButton(COMPRESSOR_BUTTON));
		}
		
			//camera motor mount
		if(oi->joyManip->GetRawButton(10))
		{
			bCameraLatch = true;
		}
		else if(oi->joyManip->GetRawButton(11))
		{
			bCameraLatch = false;
		}	
		manip->toggleCameraPosition(bCameraLatch);
	}

/**
Tests that the encoder measurements are being correctly translated into
wheel speeds
*/
void tests::measurements() {
	Drive md;
	EncoderMeasurement measure(md);

	md.setMLSpeed(400);
	md.setMRSpeed(-400);

	uint32_t prevTime;
	int i =0;
	while(true) {
		uint32_t currTime = micros();
		if (currTime - prevTime >= PERIOD_MICROS) {
			prevTime = currTime;
			measure.update();
			i++;
			if(i % 5 == 0) {
				Serial.print(prevTime);
				Serial.print("\t");
				Serial.print(measure.mVR);
				Serial.print("\t");
				Serial.println(measure.mVL);
			}
		}
	}
}

Drive* DriveManager::getDrive(std::string type, int index)
{
    Drive* drive = createDrive(type, index);
    drive->updateFreeSpace();
    drive->doQuickSMARTCheck();

    return drive;
}

    Robot() {
        log = new Log(this);

        bcd = new BcdSwitch(new DigitalInput(2,11), new DigitalInput(2,12),
        					new DigitalInput(2,13), new DigitalInput(2,14));
        bcdValue = bcd->value();

        control = new Control(
                new Joystick(1), new Joystick(2), new Joystick(3), 
                Control::Tank, log);
        control->setLeftScale(-1);
        control->setRightScale(-1);
        control->setGamepadScale(-1);

        drive = new Drive(this, 1,2);
        drive->addMotor(Drive::Left, 2, 1);
        drive->addMotor(Drive::Left, 3, 1);
        drive->addMotor(Drive::Right, 4, 1);
        drive->addMotor(Drive::Right, 5, 1);

        rightDriveEncoder = new Encoder(1,1, 1,2, true, Encoder::k2X);
        leftDriveEncoder = new Encoder(1,3, 1,4, false, Encoder::k2X);
        
        //compressor = new Compressor(2,9, 2,3); //press, relay
		//compressor->Start();

        leftClimber = new Climber(
        	this,
            "leftClimber",
            new CANJaguar(6),
            new Encoder(1,5, 1,6, true),
            ClimberDistancePerPulse,
            new DigitalInput(2,1),//lower limit switch
            new DigitalInput(2,2),//lower hook switch
            new DigitalInput(2,3) );//upper hook switch
        
        rightClimber = new Climber(
        	this,
            "rightClimber",
            new CANJaguar(7),
            new Encoder(1,7, 1,8, true),
            ClimberDistancePerPulse,
            new DigitalInput(2,4),//lower limit switch
			new DigitalInput(2,5),//lower hook switch
			new DigitalInput(2,6) );//upper hook switch	

        loaderMotor = new Relay(2,1);
        loaderSwitch = new DigitalInput(2,8);

        shooterMotor = new CANJaguar(10, CANJaguar::kVoltage);
        
        blowerMotor = new CANJaguar(9);

        cameraPivotMotor = new Servo(1,9);
        cameraElevateMotor = new Servo(1,10);

        lightRing = new Relay(2,4);
    }

	void TeleopPeriodic() {
		// Comment the next line out to disable movement
		drive->doDrive(stick->GetX(), -stick->GetY());
		intake->periodic();
		shooter->periodic();
		camSystem->periodic();
	}

void tests::servo_drive_interaction() {
	VS11 servo(3);

	servo.setGoal(0);
	delay(1000);
	servo.setGoal(M_PI/2);
	delay(1000);
	Serial.println("0");

	{
		Drive d;
		d.setMLSpeed(100);
		d.setMRSpeed(100);
		delay(1000);
		servo.setGoal(0);
		delay(1000);
		d.setMLSpeed(-100);
		d.setMRSpeed(-100);
		delay(1000);
		servo.setGoal(M_PI/2);
		delay(1000);
		d.setMLSpeed(0);
		d.setMRSpeed(0);
	}
}

bool Agent::fromBottle(Bottle b)
{
    if (!this->Object::fromBottle(b))
        return false;

    if (!b.check("belief")||!b.check("emotions"))
        return false;

    m_belief.clear();
    Bottle* beliefs = b.find("belief").asList();
    for(int i=0; i<beliefs->size() ; i++)
    {
        Bottle* bRelation = beliefs->get(i).asList();
        Relation r(*bRelation);
        m_belief.push_back(r);
    }

    m_emotions_intrinsic.clear();
    Bottle* emotions = b.find("emotions").asList();
    for(int i=0; i<emotions->size() ; i++)
    {
        Bottle* bEmo = emotions->get(i).asList();
        string emotionName = bEmo->get(0).asString().c_str();
        double emotionValue = bEmo->get(1).asDouble();
        m_emotions_intrinsic[emotionName.c_str()] = emotionValue;
    }

    m_drives.clear();
    Bottle* drivesProperty = b.find("drives").asList();
    string drivesDebug = drivesProperty->toString().c_str();
    for(int i=0; i<drivesProperty->size() ; i++)
    {
            Bottle* bD = drivesProperty->get(i).asList();
            string drivesDebug1 = bD->toString().c_str();
            Drive currentDrive;
            currentDrive.fromBottle(*bD);
            m_drives[currentDrive.name] = currentDrive;

    }
               
    Bottle* bodyProperty = b.find("body").asList();
    m_body.fromBottle(*bodyProperty);

    return true;
}

const std::string PncKeyGenerator::generate_key(const Drive& drive) {
    const auto drive_serial_number = drive.get_fru_info().get_serial_number();

    if (!drive_serial_number.has_value()) {
        throw KeyValueMissingError("Serial number is missing.");
    }

    return generate_key_base(drive) + drive_serial_number.value();
}

    void AutonomousPeriodic() { 
    	
    	currentDistance = leftDriveEncoder->GetDistance();
		shooterMotor->Set(-shooterMotorVolts);
		
    	if (currentDistance >= goalDistance){
    		drive->setLeft(0);
    		drive->setRight(0);
    		loaderMotor->Set(Relay::kForward);
    	} else {
    		drive->setLeft(.49);
    		drive->setRight(.5);
    	}
        //log->info("LRD %f %f",
        //		leftDriveEncoder->GetDistance(),
        //		rightDriveEncoder->GetDistance());
    	//log->print();
    }

/**
Tests that the encoders work when the motors are spun BY HAND
*/
void tests::encoder_wiring() {
	Drive md;
	Serial.println("Turn the motors by hand");

	uint32_t lastr, lastl;
	while(true) {
		uint32_t encr = md.getMREncoder();
		uint32_t encl = md.getMLEncoder();
		if(encr != lastr || encl != lastl) {
			Serial.print(encr);
			Serial.print(", ");
			Serial.print(encl);
			Serial.println();
		}
		delay(10);
		lastl = encl;
		lastr = encr;
	}
}

int main()
{
	Drive  d;

	cout << d.show() <<endl;
	cout << d.show_protected_data() <<endl;


	Drive *dd = new Drive;
	Base *b = dd;	//静态是Base动态是Drive
	b->v_func(); //结果是d data: 0  指针是所以调用动态函数但是参数是静态 就是base的 0

	b->no_virtual();//结果是Base类的函数


	Drive_private *pd = new Drive_private;
	cout << pd->show_protected_data() <<endl;
	
	//pd->acessy_pubilc_inhirit(); //error  不能被获取
	
}

/**
Tests that the PID controllers work
*/
void tests::motor_feedback() {
	Drive md;

	MotorController cont(md);
	EncoderMeasurement measure(md);

	uint32_t prevTime;
	int i =0;
	while(true) {
		uint32_t currTime = micros();
		if (currTime - prevTime >= PERIOD_MICROS) {
			prevTime = currTime;
			measure.update();

			cont.controlMR(0.1, measure.mVR); // right motor PI control
			cont.controlML(-0.1, measure.mVL); //left motor PI control

			i++;
			if(i % 100 == 0) {
				Serial.print(measure.encoderRCount);
				Serial.print(", ");
				Serial.println(measure.encoderLCount);
				Serial.print(": ");

				Serial.print(measure.mVR, 4);
				Serial.print(", ");
				Serial.println(measure.mVL, 4);
			}
			if(md.faulted()) {
				Serial.println("Motor fault");
				md.setMLSpeed(0);
				md.setMRSpeed(0);
				delay(100);
				md.clearFault();
			}
		}
	}

}

	void TeleopPeriodic()
	{
		manip->toggleCompressor(oi->joyDrive->GetRawButton(COMPRESSOR_BUTTON));

		//drive
		drive->drive(oi->joyDrive->GetRawAxis(FORWARD_Y_AXIS), oi->joyDrive->GetRawAxis(TURN_X_AXIS));
		//drive->shift(oi->joyDrive->GetRawButton(8), oi->joyDrive->GetRawButton(9));

		//manipulator
		// TJF: Replaced "magic numbers" with named constants
		manipArm->moveArm(oi->joyDrive->GetRawButton(UP_INTAKE_BUTTON), oi->joyDrive->GetRawButton(DOWN_INTAKE_BUTTON)); //manipArm->moveArm(oi->joyDrive->GetRawButton(6), oi->joyDrive->GetRawButton(7));
		manip->intakeBall(oi->joyDrive->GetRawButton(INTAKE_BUTTON), oi->joyDrive->GetRawButton(OUTTAKE_BUTTON), (oi->joyDrive->GetThrottle()+1)/2);

		// TJF: removed only because it doesn't work yet
		catapult->launchBall();
		printSmartDashboard(); // 01/18/2016 moved this function because it needed to be updated constantly instead of initializing it only once.

	}

    bool
    Changer::LoadCartridge(
            Drive & drive,
            Cartridge & cartridge,
            Error & error)
    {
        boost::lock_guard<boost::mutex> lock(detail_->mutex);

        int slotIDSrc;
        if ( ! cartridge.GetSlotID(slotIDSrc,error) ) {
            return false;
        }
        int slotIDDst;
        if ( ! drive.GetSlotID(slotIDDst,error) ) {
            return false;
        }

        return detail_->MoveCartridge(slotIDSrc,slotIDDst,cartridge,error);
    }

	void printSmartDashboard()
	{
		SmartDashboard::PutNumber("Drive Y-Value", oi->joyDrive->GetRawAxis(FORWARD_Y_AXIS)); //oi->getDashboard()->PutNumber("Drive Y-Value", oi->joyDrive->GetRawAxis(FORWARD_Y_AXIS));
		SmartDashboard::PutNumber("Drive X-Value", oi->joyDrive->GetRawAxis(TURN_X_AXIS));//oi->getDashboard()->PutNumber("Drive X-Value", oi->joyDrive->GetRawAxis(TURN_X_AXIS));

		SmartDashboard::PutBoolean("Compressor On?", manip->compressorState()); //oi->getDashboard()->PutBoolean("Compressor On?", manip->compressorState());
		SmartDashboard::PutNumber("Gyro Value", drive->navX->GetAngle());

		//Returns Encoder Position(with tick)
		//deleted two front motors because encoder is attached only to the back talons
		SmartDashboard::PutNumber("Left Encoder Position", drive->rearLeftMotor->GetEncPosition());
		SmartDashboard::PutNumber("Right Encoder Position", drive->rearRightMotor->GetEncPosition());

		//Returns how fast the wheel is spinning
		//deleted two front motors because encoder is attached only to the back talons
		SmartDashboard::PutNumber("Left Encoder Speed", drive->rearLeftMotor->GetEncVel()); //gives value for both back and front left encoder
		SmartDashboard::PutNumber("Right Encoder Speed", drive->rearRightMotor->GetEncVel()); //TODO: Does not return right encoder value

		SmartDashboard::PutNumber("Average Encoder Value", drive->getAvgEncVal()); //Is working 1/21/2016
	}