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); } } }
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); } }
/* * 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(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(); }
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; }
//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(); }
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 loop() { const auto sp = ppm_sp.as_float(); const auto rl = ppm_rl.as_float(); const auto speed_r = (sp-rl)*0.5f; const auto speed_l = -(sp+rl)*0.5f; Serial.print(speed_r); Serial.print(' '); Serial.println(speed_l); motor_r.set_speed(speed_r); motor_l.set_speed(speed_l); }
void setup() { Serial.begin(9600); ppm_sp.setup(); ppm_rl.setup(); motor_l.setup(); motor_r.setup(); power_h_bridge.set_output(true); }
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); }
//Actions void stop (USHORT acc) { int motorcounter =0; lmotor.setAcceleration(acc); rmotor.setAcceleration(acc); lmotor.setTargetSpeed(0); //stop rmotor.setTargetSpeed(0); while ( rmotor.getCurrentSpeed()>1 && motorcounter<stop_timeout) { motorcounter++; } }
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 setup() { Serial.begin(9600); Serial.println("Serial communication established"); /* SERVO_FRONT.attach(SERVO_FRONT_PIN); SERVO_BACK.attach(SERVO_BACK_PIN); SERVO_GRAB.attach(SERVO_GRAB_PIN); */ m1.attach(M1_EN_PIN, M1_INA_PIN, M1_INB_PIN); m2.attach(M2_EN_PIN, M2_INA_PIN, M2_INB_PIN); //roomba.start(); //roomba.safeMode(); //roomba.fullMode(); }
void feedPaper(){ while (!paperSense()){ feed.right(); } stepMotor(feedEnc, feed, 5500); feedEnc.zero(); }
void TIM8_UP_TIM13_IRQHandler(void) { // TIM_ClearITPendingBit(tim_sch.TIM, TIM_IT_Update); // counter++; if ((counter%2000)==0) { //GPIO_ToggleBits(LED4_GPIO_PORT, LED4_PIN); } // receiver.update_data(tim_throttle.DutyCycle); if (init_flag) { motor1.set_duty(receiver.stick_throttle_.convert_duty_); motor2.set_duty(receiver.stick_throttle_.convert_duty_); motor3.set_duty(receiver.stick_throttle_.convert_duty_); motor4.set_duty(receiver.stick_throttle_.convert_duty_); } }
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() { /* Threads: - incoming - Logger -> Constantly waiting for new input from grinterface.dgoossens.nl. - action - Beta -> The thread doing calculations on which moves to do next. - gpio - Biker -> The thread which executes motor movement instructions, these instructions can be queued at any time and are ran sperately by the gpio thread. */ try { // We start by starting the incoming or Logger thread, this will establish a connection grinterface.dgoossens.nl std::thread incoming(Logger::setup); while (!Logger::isConnected()) {} //Block until logger has connected, only after the logger is connected can we read the Logger::info outputs so it's important to get this connected asap // From here on we can use the logger. Logger::info("Starting up Gridbot v1.0 #75"); // We start the action and gpio threads next std::thread action(Beta::startup); std::thread gpio(Biker::setup); // We initialise all used GPIO ports, here we claim and initialise them all GPIO::initialise(4, 17, 27, 22); //7, 11, 13, 15 GPIO::initialise(18, 23, 24, 25); //12, 16, 18, 22 GPIO::initialise(6, 13, 19, 26); //31, 33, 35, 37 GPIO::initialise(12, 16, 20, 21); //32, 36, 38, 40 GPIO::initialise(10); //19 // Now we initialise the various motors and set them so they can be used. using namespace distanceunits; Motor x = Motor(4, 17, 27, 22); Motor xmirror = Motor(6, 13, 19, 26); Motor y = Motor(18, 23, 24, 25); Motor z = Motor(12, 16, 20, 21); x.setmimic(&xmirror); // We have two x motors where they mimic one another so they both drive fowards. Beta::setmotors(&x, &y, &z); action.join(); //We need to join a thread when the main thread has nothing else to do. } catch (const std::exception& e) { Logger::info(e.what()); return 0; } }
// Turn Secondary arm in to an specific angle between -110 to 110 degree: // Status: Untested void secondaryArmPosition(int angle){ if(angle > NUM_POTENTIOMETER_1_MAX_ANGLE || angle < NUM_POTENTIOMETER_1_MIN_ANGLE){ //Check if the input angle is posible return; } int diff =angle - checkSecondaryArmAngle(); //calculate the difference if(diff<5&&diff>-5){ Serial.println("Secondary Arm is not going to turn."); Serial.print("Diff:");Serial.println(diff); } else if(diff>0){ m1.forward(255); } else if(diff<0){ m1.backward(255); } while(diff>5||diff<-5){ diff =angle - checkSecondaryArmAngle(); } m1.brake(); }
// Turn the Grabber in to an specific angle between -110 to 110 degree: // Status: Untested void grabBasePosition(int angle) { if(angle > NUM_POTENTIOMETER_2_MAX_ANGLE || angle < NUM_POTENTIOMETER_2_MIN_ANGLE){ //Check if the input angle is posible return; } int diff =angle - checkGrabAngle(); //calculate the difference if(diff<5&&diff>-5){ Serial.println("Grab is not going to turn."); Serial.print("Diff:");Serial.println(diff); } else if(diff<0){ m2.backward(200); //Here is Backward!!!! } else if(diff>0){ m2.forward(200); } while(diff>5||diff<-5){ diff =angle - checkGrabAngle(); } m2.brake(); }
void stepMotor(Encoder &enc, Motor &mot, short step){ short startpos = enc.pos; short dest = startpos + step; uint8_t i=0; uint16_t error = abs(enc.pos - dest); while(abs(enc.pos - startpos) < abs(step)){ error = abs(enc.pos - dest); int16_t force = max(error*6, 70) - 2*abs(enc.speed); uint8_t power = constrain(force, 0, 112); if (force < 30){ mot.stop(); }else{ if (i < power){ if (step > 0) mot.right(); else mot.left(); }else{ mot.disable(); } } i++; } mot.stop(); _delay_ms(100); mot.disable(); }
int main(int argc, char *argv[]) { if (argc!=2) { fprintf(stderr,"please supply name of rpc port to talk to\n"); return 1; } Network yarp; Port port; if (!port.open("/motor/client")) { fprintf(stderr,"Failed to open a port, maybe run: yarpserver\n"); return 1; } //yarp.connect("/motor/client",argv[1],"text_ack"); yarp.connect("/motor/client",argv[1]); Motor motor; motor.query(port); int nj = motor.get_axes(); vector<double> poss; for (int i=0; i<nj; i++) { poss.push_back(0.5*i); } motor.set_poss(poss); for (int i=0; i<10; i++) { motor.link().stream(); motor.set_pos(0,i); motor.link().query(); double v = motor.get_enc(0); printf("Motor %d at %g of %d axes\n", 0, v, motor.get_axes()); vector<double> all = motor.get_encs(); printf(" all encs:"); for (int i=0; i<(int)all.size(); i++) { printf(" %d:%g", i, all[i]); } printf("\n"); } port.close(); return 0; }
void BallJoint::addToDescriptions(std::vector<ObjectDescription>& objectDescriptionTree, int depth) { if(this->motors.size() > 0) { SimSensor::addToDescriptions(objectDescriptionTree, depth); ObjectDescription valueDesc; std::string actuatorDescription; std::string sensorDescription; Motor* motor = motors.front(); std::vector<MotorAxisDescription>::const_iterator pos; for(int i = 0; i < motor->getNumberOfParsedAxes(); i++) { motor->getAxis(i); actuatorDescription = "motorAxis" + i; actuatorDescription += "Actuator"; sensorDescription = "motorAxis" + i; sensorDescription += "Sensor"; //Add Actuatorport: valueDesc.name = actuatorDescription; valueDesc.fullName = fullName + "." + actuatorDescription; valueDesc.depth = depth + 1; valueDesc.type = OBJECT_TYPE_ACTUATORPORT; objectDescriptionTree.push_back(valueDesc); //Add Sensorport: valueDesc.name = sensorDescription; valueDesc.fullName = fullName + "." + sensorDescription; valueDesc.depth = depth + 1; valueDesc.type = OBJECT_TYPE_SENSORPORT; objectDescriptionTree.push_back(valueDesc); } } }
//Motor(); TEST(constructorTest, defaultContructor) { Motor m; EXPECT_EQ(7,m.getDir1Pin()); EXPECT_EQ(6,m.getDir2Pin()); EXPECT_EQ(5,m.getEnablePin()); EXPECT_EQ(0,m.getSpeed()); EXPECT_TRUE( FORWARD==m.getDirection() ); }
void forward(USHORT lspeed,USHORT rspeed) { lmotor.setReverse(true); rmotor.setReverse(false); lmotor.setAcceleration(normal_acc); rmotor.setAcceleration(normal_acc); lmotor.setTargetSpeed(lspeed); rmotor.setTargetSpeed(rspeed); }
void line_follower(Motor motorA, Motor motorB) { if (sensorA.get_color() == BLANCO && sensorB.get_color() == BLANCO) { motorA.set_sentido(ADELANTE); motorB.set_sentido(ADELANTE); } else if (sensorA.get_color() == BLANCO && sensorB.get_color() != BLANCO) { motorA.set_sentido(ATRAS); motorB.set_sentido(ADELANTE); } else if (sensorA.get_color() != BLANCO && sensorB.get_color() == BLANCO) { motorA.set_sentido(ADELANTE); motorB.set_sentido(ATRAS); } }
void reverse(USHORT speed) //straight back { lmotor.setReverse(false); rmotor.setReverse(true); lmotor.setAcceleration(normal_acc); rmotor.setAcceleration(normal_acc); lmotor.setTargetSpeed(speed); rmotor.setTargetSpeed(speed); for(int del =0;del<20000;del++) { for(int eel =0;eel<reverse_delay;eel++) {} } }