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();
}
