void motor_control() { if ( runFlag == false ){ if ( p2_0 == 0 ){ Motor(FORWARD, 55, 55); runFlag = true ; } } else if ( p2_0 == 0 ){ Motor(STOP,0,0); runFlag = false ; } else if ( ad_data[1]<ad_data[2]-50 ){ // Speed down ; RightTurn(20, 10); } else if ( ad_data[1]>ad_data[2]+50 ){ // Speed down ; LeftTurn(10, 20); } /* else if (ad_data[0]!=0x01FF & ad_data[1]!=0x01FF & ad_data[2]!=0x01FF & ad_data[3]!=0x01FF){ Motor(FORWARD, 0, 0); } */ else { // Speed Up; Motor(FORWARD, 55,55); } }
/** * @brief Funktion Ausweichen * * Der Asuro benutzt den Ultraschall um die Umgebung zu scannen und weicht * jedem Hinderniss mit einer Rechtsdrehung aus und fährt weiter geradeaus. * * @param Speed Die Fahrgeschwindigkeit * @return Kein Rückgabewert(void) * */ void Ausweichen(uint16_t Speed) { sei(); /*~~~~~~~~~~*/ int pos, i; int posmarker; /*~~~~~~~~~~*/ TextAusgabe("Vor Ping"); TextAusgabe(CR); posmarker = 0; Ping(20); TextAusgabe("Nach PING"); TextAusgabe(CR); for (pos = 0; pos < 1000; pos++) { TextAusgabe("In der ersten For-schleife"); TextAusgabe(CR); /* 1000 = die maximale Entfernung */ _delay_us(10); if ((ACSR & (1 << ACI)) != 0) { TextAusgabe("Im ersten if"); TextAusgabe(CR); /* Wenn Register ACI von ACSR nicht gesetzt ist dann.. */ if (posmarker == 0) { TextAusgabe("Im zweiten if"); TextAusgabe(CR); posmarker = pos; } /* Wenn posmarker = 0 ist dann pos und posmarker gleichsetzen */ } TextAusgabe("Vor ACSR"); ACSR |= (1 << ACI); /* Das Bit ACI setzen. Damit es durch einen Interrupt gel�scht werden kann */ } if (posmarker < 200) { TextAusgabe("Im dritten if"); TextAusgabe(CR); /* Wenn ein Hinderniss im Bereich von 200 ist */ StatusLED(Rot); /* StatusLed leuchtet Rot */ Motor(BREAK, BREAK, 0, 0); /* Motor kurz anhalten */ Motor(FWD, BWD, 0, 255); /* Linkes Rad vorw�rts und Rechtes R�ckw�rts(Ausweichen) */ } else { TextAusgabe("Im else"); TextAusgabe(CR); StatusLED(Gruen); /* LED Gr�n */ Motor(FWD, FWD, Speed, Speed); /* Motoren f�hrt geradeaus */ for (i = 0; i < 100; i++) { TextAusgabe("Im zweiten for"); TextAusgabe(CR); _delay_ms(1); } /* kurze Pause */ } }
void Mobile::initMotors() { /*Two motors at the top, <> and tvo at the bottom*/ motors[0] = Motor( vec2(1, -1), vec2(0, 1)); motors[1] = Motor( vec2(-1, -1), vec2(0, 1)); motors[2] = Motor( vec2( 0, 1), vec2(1, 0)); motors[3] = Motor( vec2( 0, 1), vec2(-1, 0)); }
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; }
DalekDrive::DalekDrive(const Wheel_t wheelConfig, const UINT8 leftFront, const UINT8 rightFront, const UINT8 leftRear, const UINT8 rightRear) { #ifdef DEBUG_DRIVE printf("DalekDrive Port Constructor\n"); #endif Motor ms[] = {Motor(leftFront, LEFT_FRONT), Motor(rightFront, RIGHT_FRONT), Motor(leftRear, LEFT_REAR), Motor(rightRear, RIGHT_REAR)}; init(wheelConfig, ms); // calls alternate constructor with paramaters rearranged }
Car::Car() :m_encoder{Encoder(0), Encoder(1)}, m_servo(0), m_lcd(true), m_lcd_console(&m_lcd), m_leds{Led(0), Led(1), Led(2), Led(3)}, m_motor{Motor(0), Motor(1)}, m_uart(3, libbase::k60::Uart::Config::BaudRate::BR_115200) { SetMotorPowerLeft(0); SetMotorPowerRight(0); SetMotorDirection(true); m_servo.SetDegree(SERVO_MID_DEGREE); m_uart.EnableRx(); }
void init(){ led_init(); motor_init(); switch_init(); ground_init(); dist_init(); servo_init(); if(DEBUG) usart_init(); motor1 = Motor(&OCR1A, &MOTOR1_DIR_PORT, MOTOR1_DIR_PIN, MAX_POWER); motor2 = Motor(&OCR1B, &MOTOR2_DIR_PORT, MOTOR2_DIR_PIN, MAX_POWER); }
//============================= // MotorTest // // Turn a motor connected to port // armPort at a speed of armSpeed // for waitTime seconds //============================= void MotorTest( int portNum, int motorSpeed, float waitTime ) { FUNC_BEGIN("MotorTest"); if (waitTime < 1) printf("WARNING: waitTime is lower than 1 second: %f\n", waitTime); Motor(portNum, motorSpeed); wait( waitTime ); Motor(portNum, MOTOR_OFF); FUNC_END("MotorTest"); }
Helm::Helm() { leftEncoder = Encoder ( ENCODER1_ADDRESS, false, false ); rightEncoder = Encoder ( ENCODER2_ADDRESS, true, true ); leftMotor = Motor ( LEFT_MOTOR_PIN, LEFT_MOTOR_REVERSED, "/dev/servoblaster" ); rightMotor = Motor ( RIGHT_MOTOR_PIN, RIGHT_MOTOR_REVERSED, "/dev/servoblaster" ); currentPos.x = 0; currentPos.y = 0; currentPos.z = 0; // pthread_create( &updater, NULL, this->publish, NULL ); }
//============================ // MotorTestMultiple // // Test every motor connected on // startPort to (startPort + numMotors - 1) //============================ void MotorTestMultiple( int startPort, int numMotors, int motorSpeed, float waitTime ) { int i; FUNC_BEGIN("MotorTestMultiple"); if (waitTime < 1) printf("WARNING: waitTime is lower than 1 second: %f\n", waitTime); for (i = 0; i < numMotors; i++) Motor(startPort + i, motorSpeed); wait( waitTime ); for (i = 0; i < numMotors; i++) Motor(startPort + i, MOTOR_OFF); FUNC_END("MotorTestMultiple"); }
//============================================================================= // eFdd::Open //----------------------------------------------------------------------------- bool eFdd::Open(const char* type, const void* data, size_t data_size) { Motor(0); if(!strcmp(type, "trd")) return ReadTrd(data, data_size); if(!strcmp(type, "scl")) return ReadScl(data, data_size); if(!strcmp(type, "fdi")) return ReadFdi(data, data_size); return false; }
void _Motor( void ) { int motorNum, motorSpeed = 0; printf("Enter a motor number (1 -> 6).\n"); scanf("%d", &motorNum); printf("Enter the output strength (-127 -> 127).\n"); scanf("%d", &motorSpeed); motorNum = NumtoIO( motorNum, OUTPUT ); printf("Setting output strength on port %d to %d.\n", motorNum, motorSpeed); Motor( motorNum, motorSpeed ); }
void main() { uchar *p; p="by Jasper"; Init(); IsReset=0; IsReset=eeprom_read(100); if(!IsReset) //首次进入输入密码和显示欢迎界面 { Confirm(); LCD_clear(); delay_ms(200); LCD_write_str(5,0,"welcome"); while(*p) { LCD_write_char(i,1,*p); p++; i++; delay_ms(100); } } LCD_clear(); //正式进入系统 LCD_write_str(0,0,"1bell2led3motor"); LCD_write_str(0,1,"4fix_password"); while(1) { eeprom_write(100,0); if(select=='1') //蜂鸣器模式 { LCD_clear(); LCD_write_str(0,0,"buzz..send * or"); LCD_write_str(0,1,"shutdown to exit"); Bell(); } if(select=='2') LED(); //led模式 if(select=='3') Motor(); //电机模式 if(select=='4') Write_password(); //修改密码模式 } }
//============================================================================= // eFdd::Open //----------------------------------------------------------------------------- bool eFdd::Open(const char* type, const void* data, size_t data_size) { Motor(0); bool ok = false; if(!strcmp(type, "trd")) ok = ReadTrd(data, data_size); else if(!strcmp(type, "scl")) ok = ReadScl(data, data_size); else if(!strcmp(type, "fdi")) ok = ReadFdi(data, data_size); else if(!strcmp(type, "udi")) ok = ReadUdi(data, data_size); else if(!strcmp(type, "td0")) ok = ReadTd0(data, data_size); SAFE_CALL(disk)->Changed(false); return ok; }
void SetTapeMode(unsigned char Mode) //Handles button pressed from Dialog { TapeMode=Mode; switch (TapeMode) { case STOP: break; case PLAY: if (TapeHandle==NULL) if (!LoadTape()) TapeMode=STOP; else TapeMode=Mode; if (MotorState) Motor(1); break; case REC: if (TapeHandle==NULL) if (!LoadTape()) TapeMode=STOP; else TapeMode=Mode; break; case EJECT: CloseTapeFile(); strcpy(TapeFileName,"EMPTY"); break; } UpdateTapeCounter(TapeOffset,TapeMode); return; }
#include "PIDControlledMotor.h" #include "TwoWheelOdometryManager.h" #include "TwoWheelMotorController.h" #include "RotaryEncoder.h" #include "Config.h" RotaryEncoder<LEFT> leftEncoder(Config::Pins::LEFT_ENCODER_A, Config::Pins::LEFT_ENCODER_B, Config::ROTARY_ENCODER_RATIO); RotaryEncoder<RIGHT> rightEncoder(Config::Pins::RIGHT_ENCODER_A, Config::Pins::RIGHT_ENCODER_B, Config::ROTARY_ENCODER_RATIO); PIDControlledMotor leftMotor(Config::Dimensions::WHEEL_RADIUS, Motor(Config::Pins::LEFT_MOTOR_DIRECTION, Config::Pins::LEFT_MOTOR_SPEED), &leftEncoder); PIDControlledMotor rightMotor(Config::Dimensions::WHEEL_RADIUS, Motor(Config::Pins::RIGHT_MOTOR_DIRECTION, Config::Pins::RIGHT_MOTOR_SPEED), &rightEncoder); TwoWheelMotorController motorController(&leftMotor, &rightMotor, Config::Dimensions::ROBOT_WIDTH); void loop() { motorController.OnTick(); } int main() { motorController.SetVelocities(10, 0.2); // This should make the robot drive in a circle
string FabAtHomePrinter::loadConfigFile(string filePath) { //Clear previously loaded data. axes.clear(); motors.clear(); tool.bays.clear(); XMLParser parser; string result = parser.load(filePath); if(result.compare("") != 0) { return result; } //Load constants. COM_PORT = Util::assertType<unsigned int>(parser.text("fabAtHomePrinter 0\\electronics 0\\comPort 0")); NUM_MODULES = Util::assertType<unsigned int>(parser.text("fabAtHomePrinter 0\\electronics 0\\numModules 0")); BAUD_RATE = Util::assertType<unsigned int>(parser.text("fabAtHomePrinter 0\\electronics 0\\baudRate 0")); X_Y_Z_GROUP_ADDRESS = (byte)Util::assertType<unsigned int>(parser.text("fabAtHomePrinter 0\\electronics 0\\xyzGroupAddress 0")); PLATFORM_DELTA = Util::assertType<double>(parser.text("fabAtHomePrinter 0\\motion 0\\platformDelta 0")); OLD_MSPS = Util::assertType<double>(parser.text("fabAtHomePrinter 0\\motion 0\\oldMsps 0")); //Load motors. unsigned int count = parser.count("fabAtHomePrinter 0\\electronics 0\\motor"); for(unsigned int i = 0; i < count; ++i) { string base = "fabAtHomePrinter 0\\electronics 0\\motor "+Util::toString(i)+"\\"; string name = parser.text(base+"name 0"); byte address = (byte)Util::assertType<unsigned int>(parser.text(base+"address 0")); double countsPerDistanceUnit = Util::assertType<double>(parser.text(base+"countsPerDistanceUnit 0")); short kp = Util::assertType<unsigned short>(parser.text(base+"kp 0")); short kd = Util::assertType<unsigned short>(parser.text(base+"kd 0")); short ki = Util::assertType<unsigned short>(parser.text(base+"ki 0")); short il = Util::assertType<unsigned short>(parser.text(base+"il 0")); byte ol = (byte)Util::assertType<unsigned int>(parser.text(base+"ol 0")); byte cl = (byte)Util::assertType<unsigned int>(parser.text(base+"cl 0")); short el = Util::assertType<unsigned short>(parser.text(base+"el 0")); byte sr = (byte)Util::assertType<unsigned int>(parser.text(base+"sr 0")); byte db = (byte)Util::assertType<unsigned int>(parser.text(base+"db 0")); double ticksPerSecond = Util::assertType<double>(parser.text(base+"ticksPerSecond 0")); motors[name] = Motor(name,address,countsPerDistanceUnit,ticksPerSecond,kp,kd,ki,il,ol,cl,el,sr,db); } //Load axes. count = parser.count("fabAtHomePrinter 0\\axis"); for(unsigned int i = 0; i < count; ++i) { string base = "fabAtHomePrinter 0\\axis "+Util::toString(i)+"\\"; string name = parser.text(base+"name 0"); string motorName = parser.text(base+"motorName 0"); map<string, Motor, LessThanString>::iterator motor = motors.find(motorName); if(motor == motors.end()) { return "Axis "+name+" references motor "+motorName+" which has not been loaded."; } if(name.compare("Y") == 0 || name.compare("Z") == 0) { motor->second.setReversed(true); } axes[name] = Axis(name,&(motor->second)); } //Check that axes named X, Y, and Z were loaded. if(axes.find("X") == axes.end()) { return "Must load an axis named X."; } if(axes.find("Y") == axes.end()) { return "Must load an axis named Y."; } if(axes.find("Z") == axes.end()) { return "Must load an axis named Z."; } //Load bays. count = parser.count("fabAtHomePrinter 0\\tool 0\\bay"); for(unsigned int i = 0; i < count; ++i) { string base = "fabAtHomePrinter 0\\tool 0\\bay "+Util::toString(i)+"\\"; string name = parser.text(base+"name 0"); string motorName = parser.text(base+"motorName 0"); if(motors.find(motorName) == motors.end()) { return "Bay "+name+" references motor "+motorName+" which has not been loaded."; } double x = Util::assertType<double>(parser.text(base+"location 0\\x 0")); double y = Util::assertType<double>(parser.text(base+"location 0\\y 0")); double z = Util::assertType<double>(parser.text(base+"location 0\\z 0")); tool.bays[name] = Bay(name,Point(x,y,z),&(motors.find(motorName)->second)); } //Check that at least one bay is loaded. if(tool.bays.size() == 0) { return "Must load at least one bay."; } return ""; }
Motor::Motor(int id) : ID(id) { Motor(); }