C++ Motor类代码示例

void BallJoint::addToLists(std::vector<Sensorport*>& sensorportList,
                       std::vector<Actuatorport*>& actuatorportList,
                       std::vector<Actuator*>& actuatorList) 
{
  if(this->motors.size() > 0)
  {
    std::string actuatorDescription;
    std::string sensorDescription;
    Motor* motor = motors.front();
    std::vector<MotorAxisDescription>::const_iterator pos;

    for(int i = 0; i < motor->getNumberOfParsedAxes(); i++)
    {
      MotorAxisDescription* axis = motor->getAxis(i);
      actuatorDescription = "motorAxis" + i;
      actuatorDescription += "Actuator";
      sensorDescription = "motorAxis" + i;
      sensorDescription += "Sensor";

      Actuatorport* value = new Actuatorport
        (actuatorDescription, this, i, axis->minValue, axis->maxValue);
      actuatorportList.push_back(value);
      Sensorport* valueSensor = new Sensorport
        (sensorDescription, i, doubleSensor, this, axis->minValue, axis->maxValue);
      sensorportList.push_back(valueSensor);
      actuatorList.push_back(this);
    }
  }
}

/*
 * Updates the fields of current motor and sets the Heading
 * of its pins
 */
void MotorController::init()
{   
  // Attach motors
  leftMotor.attach(LEFT_MOTOR_IN_A_PIN, LEFT_MOTOR_IN_B_PIN, LEFT_MOTOR_PWM_PIN);
  rightMotor.attach(RIGHT_MOTOR_IN_A_PIN, RIGHT_MOTOR_IN_B_PIN, RIGHT_MOTOR_PWM_PIN);
  
  // Attach encoders
  leftEncoder.attach(LEFT_ENCODER_INT_PIN, LEFT_ENCODER_DIGITAL_PIN);
  rightEncoder.attach(RIGHT_ENCODER_INT_PIN, RIGHT_ENCODER_DIGITAL_PIN);  

  // Consider the starting point of the robot
  // as its origin
  _position.setX(0);
  _position.setY(0);

  // Suppose robot is heading along Y axis
  _heading = 90;

  // Initialize the number of commands to 0
  _commandsCount = 0;
  _processingNewCommand = true;
  
  _leftMotorPosition = 0;
  _leftMotorLastPosition = 0;
  _rightMotorPosition = 0;
  _rightMotorLastPosition = 0;
  _leftMotorSpeed = 0;
  _rightMotorSpeed = 0;
  _leftMotorPWM = 0;
  _rightMotorPWM = 0;

  // Set vacuum pump pin as output
  pinMode(VACUUM_PIN, OUTPUT);
  stopVacuum();
}

int main()
{
	string doorCond = "";
	int data[] = {100, 101, 105, 110, 210, 100, 102, 110, 150, 100};
	
	Sensor sensor;
	Motor motor;
	Door_latch door_latch;

	printf("Please indicate whether the door is closed or open\nby typing 'closed' or 'open' below:\n");
	cin >> doorCond;
	printf("\n");

	if(door_latch.DoorClosed(doorCond)){	//if door is closed
		sensor.ReadData(data);				//reads the data
		if(sensor.Calibrate()){				//once the calibration is successful, operate the motor
			printf("calibration successful!\n");
			motor.MoveMotor(sensor.motor_position());
		}else
			printf("calibration unsuccessful, please provide another set of data\n");
	}else									//if the door is open, reset the motor
	{
		motor.ResetMotor();
		printf("Door is open\n");
	}
	system("PAUSE");
}

XERCES_CPP_NAMESPACE_USE
#endif	


  

int main(int argc, char* argv[])
{
	char* nombre_fichero="proyecto.xml";
	
	vector<Elemento*> v;
	
	Parser* elParser= new Parser ();
	
	if (elParser->esValido (nombre_fichero) ) 
		cout << "El fichero es XML valido" << endl;
	v=elParser->extraerElementos();
	long i=v.capacity();
	cout << "El vector contiene " << i << endl;
	
	Motor* m;
	m=(Motor*) v.at(0);
	cout << m->getEntradaGiro1();
  // Poner aqui el resto de delete's, cuando sean necesarios
  return 0;
}

webots::Robot::Robot() {
  if (cInstance == NULL)
    cInstance = this;
  else {
    cerr << "Only one instance of the Robot class should be created" << endl;
    exit(-1);
  }

  initDarwinOP();
  initDevices();
  gettimeofday(&mStart, NULL);
  mPreviousStepTime = 0.0;
  mKeyboardEnable = false;
  mKeyboard = new Keyboard();

  // Load TimeStep from the file "config.ini"
  minIni ini("config.ini");
  LoadINISettings(&ini, "Robot Config");
  if (mTimeStep < 16) {
    cout << "The time step selected of " << mTimeStep << "ms is very small and will probably not be respected." << endl;
    cout << "A time step of at least 16ms is recommended." << endl;
  }

  mCM730->MakeBulkReadPacketWb(); // Create the BulkReadPacket to read the actuators states in Robot::step
  
  // Unactive all Joints in the Motion Manager //
  std::map<const std::string, int>::iterator motorIt;
  for (motorIt = Motor::mNamesToIDs.begin() ; motorIt != Motor::mNamesToIDs.end(); ++motorIt ) {
    ::Robot::MotionStatus::m_CurrentJoints.SetEnable((*motorIt).second, 0);
    ::Robot::MotionStatus::m_CurrentJoints.SetValue((*motorIt).second, ((Motor *) mDevices[(*motorIt).first])->getGoalPosition());
  }


  // Make each motors go to the start position slowly
  const int msgLength = 5; // id + Goal Position (L + H) + Moving speed (L + H)
  int value = 0, changed_motors = 0, n = 0;
  int param[20 * msgLength];
  
  for (motorIt = Motor::mNamesToIDs.begin() ; motorIt != Motor::mNamesToIDs.end(); ++motorIt ) {
    Motor *motor = static_cast <Motor *> (mDevices[(*motorIt).first]);
    int motorId = (*motorIt).second;
    if (motor->getTorqueEnable() && !(::Robot::MotionStatus::m_CurrentJoints.GetEnable(motorId))) {
      param[n++] = motorId;  // id
      value = motor->getGoalPosition(); // Start position
      param[n++] = ::Robot::CM730::GetLowByte(value);
      param[n++] = ::Robot::CM730::GetHighByte(value);
      value = 100; // small speed 100 => 11.4 rpm => 1.2 rad/s
      param[n++] = ::Robot::CM730::GetLowByte(value);
      param[n++] = ::Robot::CM730::GetHighByte(value);
      changed_motors++;
    }
  }
  mCM730->SyncWrite(::Robot::MX28::P_GOAL_POSITION_L, msgLength, changed_motors , param);
  usleep(2000000); // wait a moment to reach start position

  // Switch LED to GREEN
  mCM730->WriteWord(::Robot::CM730::ID_CM, ::Robot::CM730::P_LED_HEAD_L, 1984, 0);
  mCM730->WriteWord(::Robot::CM730::ID_CM, ::Robot::CM730::P_LED_EYE_L, 1984, 0);
}

void loop(void) {
	SerialUSB.println("Motor test loop");
	motor.setPower(0.5f);
	delay(2500);
	motor.setPower(0f);
	delay(2500);
	motor.setPower(1.0f);
	delay(2500);
}

void Aircraft_Init(void) {
  GPIO_InitTypeDef  GPIO_InitStructure;
  // LCD
#ifdef _DEBUG_WITH_LCD
  RCC_AHB1PeriphClockCmd(RCC_AHB1Periph_GPIOD, ENABLE);
  GPIO_InitStructure.GPIO_Pin = GPIO_Pin_7;
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
  GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;
  GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_UP;
  GPIO_Init(GPIOD, &GPIO_InitStructure);
  GPIO_ResetBits(GPIOD , GPIO_Pin_7);    //CS=0;
  LCD_Initializtion();
  LCD_Clear(Blue);
#endif
  // 捕获器
  tim_throttle.mode_pwm_input(THROTTLE_PIN);
  tim_pitch.mode_pwm_input(PITCH_PIN);
  tim_yaw.mode_pwm_input(YAW_PIN);
  tim_roll.mode_pwm_input(ROLL_PIN);
  // 调度器
  tim_sch.mode_sch();
  // 接收机
  receiver.stick_throttle_ = Stick(0.0502,0.0999,0,NEGATIVE_LOGIC);
  // 油门最高点，听到确认音 
  motor1 = Motor(PWM_FREQ,MAX_DUTY,MOTOR1_PWM_TIM,MOTOR1_PWM_CH,MOTOR1_PWM_PIN);
  motor2 = Motor(PWM_FREQ,MAX_DUTY,MOTOR2_PWM_TIM,MOTOR2_PWM_CH,MOTOR2_PWM_PIN);
  motor3 = Motor(PWM_FREQ,MAX_DUTY,MOTOR3_PWM_TIM,MOTOR3_PWM_CH,MOTOR3_PWM_PIN);
  motor4 = Motor(PWM_FREQ,MAX_DUTY,MOTOR4_PWM_TIM,MOTOR4_PWM_CH,MOTOR4_PWM_PIN);
  // 电调接上电池，等待2S
  Delay(DELAY_1S);
  Delay(DELAY_1S);  
  
  // 以防万一，再次延迟2S
  // Delay(DELAY_1S);
  // Delay(DELAY_1S);
  // 油门推到最低，等待1S
  motor1.set_duty(MIN_DUTY);
  motor2.set_duty(MIN_DUTY);
  motor3.set_duty(MIN_DUTY);
  motor4.set_duty(MIN_DUTY);
  Delay(DELAY_1S);
  Delay(DELAY_1S);
  Delay(DELAY_1S);
  // 油门最低点确认音，可以起飞
  
  // LED4
  RCC_AHB1PeriphClockCmd(LED4_GPIO_CLK, ENABLE);
  GPIO_InitStructure.GPIO_Pin = LED4_PIN;
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_OUT;
  GPIO_InitStructure.GPIO_OType = GPIO_OType_PP;
  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;
  GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;
  GPIO_Init(LED4_GPIO_PORT, &GPIO_InitStructure);
  GPIO_ResetBits(LED4_GPIO_PORT,LED4_PIN);
  init_flag = 1;
}

void MotorControl::move(float power, float move_angle) {
    power_ = power;
    move_angle_ = move_angle;

    double cosine = acceleration_rate_ * power * cos(move_angle * 0.0174533);
    double sine   = acceleration_rate_ * power * sin(move_angle * 0.0174533);

    motor1->run(omega + ( 0.35 * sine - 0.6062177826491071 * cosine));
    motor2->run(omega + (-0.70 * sine));
    motor3->run(omega + ( 0.35 * sine + 0.6062177826491071 * cosine));
}

void MotorController::fuzzyMove(double left, double right)
{
  fuzzyCompute();
  
  #ifdef MOTOR_CONTROLLER_DEBUG
  Serial.print("Left motor PWM: ");
  Serial.print(_leftMotorPWM);
  Serial.print(" Right motor PWM: ");
  Serial.println(_rightMotorPWM);
  Serial.println("");
  #endif
  leftMotor.turn(_leftMotorPWM * left);
  rightMotor.turn(_rightMotorPWM * right);
}

Motor * BlueprintInstance::detachMotor(unsigned int i)
{
    assert(i < mMotorList.size());
    Motor * detached = mMotorList[i];

    // detach from list
    mMotorList.erase(mMotorList.begin() + i);

    // detach from map
    map<string, Motor*>::iterator iter =
        mMotorMap.find(detached->getName());
    mMotorMap.erase(iter);

    return detached;
}

void feedPaper(){
	while (!paperSense()){
		feed.right();
	}
	stepMotor(feedEnc, feed, 5500);
	feedEnc.zero();
}

void init( const std::string& hostname)
{
	// Initialize the actors
	motor1.setMotorNumber( 0 );

	motor2.setMotorNumber( 1 );

	motor3.setMotorNumber( 2 );

	// Connect
	std::cout << "Connecting to  "<< hostname << " ... " << std::endl;
	com.setAddress( hostname.c_str() );

	com.connect();

	odometry.set(0, 0, 0);

	std::cout << std::endl << "Connected" << std::endl;
}

int main() {
  init();
  leds_on();
  
  wait_s(5); // regulaminowy czas

  for(;;){
    if(DEBUG) debug();
        
    if(switch1_pressed()){
      leds_negate();
    }
    
    if(queue.head){
      move = queue.pop(ITIME);
      motor1.set_power(move.m1);
      motor2.set_power(move.m2);
    } else {
      leds_off();
      // szukanie
      motor1.set_power(0);
      motor2.set_power(0);
    }
    wait_ms(ITIME);
  }
}

int main(int argc, char** argv) {
  (void) argc;
  (void) argv;
  
  Aversive::init();
  
  Task t1([]() {
      if(enc_l.getValue() >= 10000 && MOT_L >= 0) {
	mot_l.setValue(-10);
	//io << "stop left !\n";
	//Aversive::stop();
      }
      if(enc_r.getValue() >= 50000 && MOT_R >= 0) {
	mot_r.setValue(-10);
	//io << "stop right !\n";
      }
      //ClientThread::instance().sendMessage(ClientThread::INFO, "TEST");
      
    });
  t1.setPeriod(10000);
  t1.setRepeat();

  sched.addTask(t1);

  mot_l.setValue(100);
  mot_r.setValue(100);

  while(Aversive::sync()) {
    //io << "test " << ENC_L << " " << ENC_R << "\n";
  }
  
  Aversive::setReturnCode(0);
  return Aversive::exit();
}

//The setup function is called once at startup of the sketch
void setup()
{
	Serial.begin(57600);
//	while (!Serial) {
//		;
//	}
	setupLogger.log("Start");
	motor1.setup();
	motor2.setup();
}