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++)
{}
}
}
