void init_encoders() { rf_encoder = encoderInit(ENC_RF_TOP,ENC_RF_BOT,0); lf_encoder = encoderInit(ENC_LF_TOP,ENC_LF_BOT,0); lb_encoder = encoderInit(ENC_LB_TOP,ENC_LB_BOT,0); rb_encoder = encoderInit(ENC_RB_TOP,ENC_RB_BOT,0); }
void initDriveEncoders() { // Init right encoder, reverse encDriveBR = encoderInit(DIGITAL_ENC_DRIVE_BR_TOP, DIGITAL_ENC_DRIVE_BR_BOT, true); // Init left encoder, reverse encDriveBL = encoderInit(DIGITAL_ENC_DRIVE_BL_TOP, DIGITAL_ENC_DRIVE_BL_BOT, true); }
/** * Runs user initialization code. * * This function will be started in its own task with the default priority and stack size once when the robot is starting up. It is possible that the VEXnet communication link may not be fully established at this time, so reading from the VEX Joystick may fail. * * This function should initialize most sensors (gyro, encoders, ultrasonics), LCDs, global variables, and IMEs. * * This function must exit relatively promptly, or the operatorControl() and autonomous() tasks will not start. An autonomous mode selection menu like the pre_auton() in other environments can be implemented in this task if desired. */ void initialize() { robotDrive = initDrive(initPantherMotor(8,0), initPantherMotor(1,1), initPantherMotor(9,0), initPantherMotor(2,1), initPantherMotor(10,0), initPantherMotor(3,1), encoderInit(1, 2, 0), encoderInit(3,4,0)); robotIntake = initIntake(initPantherMotor(5,0), initPantherMotor(6,0)); robotShooter = initShooter(initPantherMotor(4,0), initPantherMotor(7,0), 100); }
/* * Runs user initialization code. This function will be started in its own task with the default * priority and stack size once when the robot is starting up. It is possible that the VEXnet * communication link may not be fully established at this time, so reading from the VEX * Joystick may fail. * * This function should initialize most sensors (gyro, encoders, ultrasonics), LCDs, global * variables, and IMEs. * * This function must exit relatively promptly, or the operatorControl() and autonomous() tasks * will not start. An autonomous mode selection menu like the pre_auton() in other environments * can be implemented in this task if desired. */ void initialize() { lcdInit(uart1); lcdClear(uart1); speakerInit(); eLeft = encoderInit(ENC_L_A, ENC_L_B,false); eRight = encoderInit(ENC_R_A, ENC_R_B,true); }
/* * Runs user initialization code. This function will be started in its own task with the default * priority and stack size once when the robot is starting up. It is possible that the VEXnet * communication link may not be fully established at this time, so reading from the VEX * Joystick may fail. * * This function should initialize most sensors (gyro, encoders, ultrasonics), LCDs, global * variables, and IMEs. * * This function must exit relatively promptly, or the operatorControl() and autonomous() tasks * will not start. An autonomous mode selection menu like the pre_auton() in other environments * can be implemented in this task if desired. */ void initialize() { imecount=imeInitializeAll(); r_encoder = encoderInit(ENC_RIGHT_TOP,ENC_RIGHT_BOT,1); l_encoder = encoderInit(ENC_LEFT_TOP,ENC_LEFT_BOT,0); pinMode(CON_SWITCH,INPUT); //lcdinit(LCD_PORT); }
/** * Runs user initialization code. * * This function will be started in its own task with the default priority and stack size once when the robot is starting up. It is possible that the VEXnet communication link may not be fully established at this time, so reading from the VEX Joystick may fail. * * This function should initialize most sensors (gyro, encoders, ultrasonics), LCDs, global variables, and IMEs. * * This function must exit relatively promptly, or the operatorControl() and autonomous() tasks will not start. An autonomous mode selection menu like the pre_auton() in other environments can be implemented in this task if desired. */ void initialize() { robotDrive = initDrive(initPantherMotor(8,0), initPantherMotor(1,1), initPantherMotor(9,0), initPantherMotor(2,1), initPantherMotor(10,0), initPantherMotor(3,1), encoderInit(1, 2, 0), encoderInit(3,4,0)); robotIntake = initIntake(initPantherMotor(5,0), initPantherMotor(6,0)); PIDController shooterPID = initPIDController(0, 0, 0, .05, 0, 100); robotShooter = initShooter(shooterPID, initPantherMotor(4,0), initPantherMotor(7,0), 3000, 0, 0); }
/** * Runs user initialization code. * * This function will be started in its own task with the default priority and stack size once when the robot is starting up. It is possible that the VEXnet communication link may not be fully established at this time, so reading from the VEX Joystick may fail. * * This function should initialize most sensors (gyro, encoders, ultrasonics), LCDs, global variables, and IMEs. * * This function must exit relatively promptly, or the operatorControl() and autonomous() tasks will not start. An autonomous mode selection menu like the pre_auton() in other environments can be implemented in this task if desired. */ void initialize() { imeInitializeAll(); robotDrive = initDrive(initPantherMotor(8,0), initPantherMotor(1,1), initPantherMotor(9,0), initPantherMotor(2,1), initPantherMotor(10,0), initPantherMotor(3,1), encoderInit(1, 2, 0), encoderInit(3,4,0)); PIDController shooterPID = initPIDController(.0125, 0, 0, .028, 0, 100); robotShooter = initShooter(shooterPID, initPantherMotor(4,0), initPantherMotor(7,0), 2800, 0, 0); }
void encoderTest(void) { // initialize the encoders encoderInit(); // print a little intro message so we know things are working rprintf("\r\nWelcome to the encoder test!\r\n"); // report encoder position continuously while(1) { rprintfProgStrM("Encoder0: "); // print encoder0 position // use base-10, 10 chars, signed, pad with spaces rprintfNum(10, 10, TRUE, ' ', encoderGetPosition(0)); rprintfProgStrM(" Encoder1: "); // print encoder1 position // use base-10, 10 chars, signed, pad with spaces rprintfNum(10, 10, TRUE, ' ', encoderGetPosition(1)); // print carriage return and line feed rprintfCRLF(); } }
void armHelixUp (double spikeNumber) { const Encoder TOWER_ENCODER = encoderInit (2,3,false); armUpDead(); // power all lift motors to max motorSet (SWING_MOTOR, SWING_MOTOR_SPEED); bool done =false, swingDone =false, heightDone =false; int towerCount=0, swingCount=0; while(!done) { swingCount = analogRead (SWING_POT_PIN); //4095 is 5 volts towerCount = encoderGet(TOWER_ENCODER);//documentation claims 360 ticks in 1 revolution if (towerCount > SPIKE_OFFSET * spikeNumber) { stopArm(); printf("Tower raised, might fall, motor stopped\n"); heightDone = true; } if (swingCount > SWING_ANGLE) { motorSet (SWING_MOTOR, 0); printf("Arm swung, may be wrong angle\n"); swingDone=true; } done = swingDone && heightDone; } }
///Move to loading height void armHelixDown() { motorSet (SWING_MOTOR, -SWING_MOTOR_SPEED); armDownDead(); // power all lift motors to max bool done =false, swingDone =false, heightDone =false; int towerCount=0, swingCount=0; const Encoder TOWER_ENCODER = encoderInit (TOWER_ENCODER_TOP_CHANNEL,TOWER_ENCODER_BOTTOM_CHANNEL,false); while(!done) { swingCount = analogRead (SWING_POT_PIN); //4095 is 5 volts towerCount = encoderGet(TOWER_ENCODER);//documentation claims 360 ticks in 1 revolution if (towerCount < SPIKE_OFFSET*1.25) //1.25 is an estimate for the load height { stopArm(); printf("Tower lowered to load position\n"); heightDone = true; } if (swingCount < 100) //TODo no measurements for pot yet { motorSet (SWING_POT_PIN, 0); printf("Arm swung, may be wrong angle\n"); swingDone=true; } done = swingDone && heightDone; } }
Sensor createSensor(SensorType type, int port) { static bool initIME = false; Sensor retSen = {.type = type, .port = port}; switch (type) { case SHAFT_ENCODER: retSen.enc = encoderInit(port, port + 1, false); if (DEBUG) { print("encoder init"); if (retSen.enc == NULL) { print(" FAILED"); } getchar(); } break; case GYROSCOPE: retSen.gyr = gyroInit(port, 0); break; case ULTRASONIC: retSen.ult = ultrasonicInit(port, port + 1); break; case IME: if (!initIME) { printDebug("Init IME!"); // imeReinitialize(); initIME = true; printDebug("Done"); } break; default: break; } return retSen; }
// RobotMode robotMode; void initialize() { //gyro = gyroInit(1, 0); // robotMode = STANDARD_MODE; analogCalibrate(3); claw.status = HOLDING; claw.holdTarget = clawGetPosition(); claw.autoOpenPos = 0; claw.autoOpenTrigger -1; taskCreate(clawTask, TASK_DEFAULT_STACK_SIZE, NULL, TASK_PRIORITY_DEFAULT); taskCreate(armTask, TASK_DEFAULT_STACK_SIZE, NULL, TASK_PRIORITY_DEFAULT); leftEncoder = encoderInit(LEFT_ENC_1,LEFT_ENC_2,1); rightEncoder = encoderInit(RIGHT_ENC_1,RIGHT_ENC_2,0); }
RedEncoder initRedEncoder(int top, int bottom, int inverted) { Encoder encoder = encoderInit(top, bottom, inverted); RedEncoder newEncoder = {encoder, 0, millis(), 125, 0.0}; return newEncoder; }
int main(void) { /* USER CODE BEGIN 1 */ /* USER CODE END 1 */ /* MCU Configuration----------------------------------------------------------*/ /* Reset of all peripherals, Initializes the Flash interface and the Systick. */ HAL_Init(); /* Configure the system clock */ SystemClock_Config(); /* Initialize all configured peripherals */ MX_GPIO_Init(); MX_DMA_Init(); MX_ADC1_Init(); MX_I2C1_Init(); MX_RTC_Init(); MX_SPI2_Init(); MX_TIM1_Init(); MX_TIM2_Init(); MX_TIM3_Init(); MX_TIM4_Init(); MX_USART3_UART_Init(); /* USER CODE BEGIN 2 */ encoderInit(); pwmInit(); // adcInit(); uartInit(); timInterruptInit(); gyroInit(GYROHIGH); calibrateGyro(); // rotaryRight(800); // HAL_Delay(500); // rotaryLeft(800); drive(VEL); /* USER CODE END 2 */ /* Infinite loop */ /* USER CODE BEGIN WHILE */ while (1) { /* USER CODE END WHILE */ /* USER CODE BEGIN 3 */ } /* USER CODE END 3 */ }
void initialize() { //initialize base encoders yellowDriveEncoder = encoderInit(8, 9, true); greenDriveEncoder = encoderInit(6, 7, false); //initialize flywheel encoders yellowFlywheelEncoder = encoderInit(3, 4, false); greenFlywheelEncoder = encoderInit(11, 12, false); tbhStarted = 0; //initialize lcd screen lcdInit(uart1 ); lcdClear(uart1 ); lcdSetBacklight(uart1, true); //run autonomous selection menu when we are done initializing lcdAutoSelection(); }
int main(void) { WDTCTL = WDTPW + WDTHOLD; P1DIR |= LED1 + LED2; P1OUT &= ~(LED1 + LED2); encoderInit(); _BIS_SR(LPM4_bits + GIE); //set low power mode 4 }
/* * Call this from the default Initialize function. * After, be sure to reinitialize each motor you will be using on your robot. */ void riceBotInitialize() { Motor *MOTDTFrontRight = newMotor(); Motor *MOTDTFrontMidRight = newMotor(); Motor *MOTDTMidRight = newMotor(); Motor *MOTDTBackRight = newMotor(); Motor *MOTDTFrontLeft = newMotor(); Motor *MOTDTFrontMidLeft = newMotor(); Motor *MOTDTMidLeft = newMotor(); Motor *MOTDTBackLeft = newMotor(); Motor *MOTARMFront = newMotor(); Motor *MOTARMRight = newMotor(); Motor *MOTARMLeft = newMotor(); Motor *MOTARMTopRight = newMotor(); Motor *MOTARMBottomRight = newMotor(); Motor *MOTARMTopLeft = newMotor(); Motor *MOTARMBottomLeft = newMotor(); Motor *MOTCOL = newMotor(); printf("%d\n\r", imeInitializeAll()); imeReset(IMEARMLEFT); imeReset(IMEARMRIGHT); encDTLeft = encoderInit(0, 0, false); encDTRight = encoderInit(0, 0, false); encARMLeft = encoderInit(0, 0, false); encARMRight = encoderInit(0, 0, false); Ricencoder *ENCDT = newEncoder(); Ricencoder *ENCARM = newEncoder(); // gyro = gyroInit(0, 196); // gyroVal = gyroGet(gyro); analogCalibrate(POTARMLeft); analogCalibrate(POTARMRight); analogCalibrate(POTARMFront); Pid *PidARMLeft = newPid(); Pid *PidARMRight = newPid(); Pid *PidARMFront = newPid(); }
void autonomous() { Encoder leftEnc; //Sets encoder variable Encoder rightEnc; leftEnc = encoderInit(1, 2, 0); //Points to encoders rightEnc= encoderInit(3, 4, 0); encoderReset(leftEnc); //Resets Encoder encoderReset(rightEnc); int leftVal; //Initializes value variable int rightVal; leftVal = encoderGet(leftEnc); //Gets initial value for encoder rightVal= encoderGet(rightEnc); motorSet(1, 127); //Motors Forward motorSet(2, 127); motorSet(9, -127); motorSet(10, 127); while(leftVal <= 360 && rightVal <= 360) { leftVal = encoderGet(leftEnc); //Gets encoder value rightVal= encoderGet(rightEnc); } motorSet(1, 0); //Stops Motors motorSet(2, 0); motorSet(9, 0); motorSet(10, 0); }
Flywheel *flywheelInit(FlywheelSetup setup) { Flywheel *flywheel = malloc(sizeof(Flywheel)); flywheel->target = 0.0f; flywheel->measured = 0.0f; flywheel->measured = 0.0f; flywheel->derivative = 0.0f; flywheel->integral = 0.0f; flywheel->error = 0.0f; flywheel->action = 0.0f; flywheel->lastAction = 0.0f; flywheel->lastError = 0.0f; flywheel->firstCross = true; flywheel->reading = 0; flywheel->microTime = micros(); flywheel->timeChange = 0.0f; flywheel->pidKp = setup.pidKp; flywheel->pidKi = setup.pidKi; flywheel->pidKd = setup.pidKd; flywheel->tbhGain = setup.tbhGain; flywheel->tbhApprox = setup.tbhApprox; flywheel->bangBangValue = setup.bangBangValue; flywheel->gearing = setup.gearing; flywheel->encoderTicksPerRevolution = setup.encoderTicksPerRevolution; flywheel->smoothing = setup.smoothing; flywheel->ready = true; flywheel->delay = FLYWHEEL_READY_DELAY; flywheel->allowReadify = true; flywheel->controllerType = CONTROLLER_TYPE_PID; flywheel->targetMutex = mutexCreate(); flywheel->task = NULL; //flywheel->task = taskCreate(task, 1000000, flywheel, FLYWHEEL_READY_PRIORITY); // TODO: What stack size should be set? flywheel->encoder = encoderInit(setup.encoderPortTop, setup.encoderPortBottom, setup.encoderReverse); flywheel->motorChannels[0] = setup.motorChannels[0]; flywheel->motorChannels[1] = setup.motorChannels[1]; flywheel->motorChannels[2] = setup.motorChannels[2]; flywheel->motorChannels[3] = setup.motorChannels[3]; flywheel->motorReversed[0] = setup.motorReversed[0]; flywheel->motorReversed[1] = setup.motorReversed[1]; flywheel->motorReversed[2] = setup.motorReversed[2]; flywheel->motorReversed[3] = setup.motorReversed[3]; return flywheel; }
Drive initDrive(int leftMotor, int leftMotorInverted, int rightMotor, int rightMotorInverted, int leftEncoderTopPort, int leftEncoderBottomPort, int leftEncoderInverted, int rightEncoderTopPort, int rightEncoderBottomPort, int rightEncoderInverted, int gyroPort, int echoPort, int pingPort) { PantherMotor left = initPantherMotor(leftMotor, leftMotorInverted); PantherMotor right = initPantherMotor(rightMotor, rightMotorInverted); Encoder leftEncoder = encoderInit(leftEncoderTopPort, leftEncoderBottomPort, leftEncoderInverted); Encoder rightEncoder = encoderInit(rightEncoderTopPort, rightEncoderBottomPort, rightEncoderInverted); Gyro gyro = gyroInit(gyroPort, 0); Ultrasonic newSonar = ultrasonicInit(echoPort, pingPort); Drive newDrive = {left, right, leftEncoder, rightEncoder, gyro, newSonar}; return newDrive; }
//=========================================================================== int cDriverSensoray626::open() { // check if device is not already opened if (m_systemReady) { return (0); } // number of encoders used. for (int i = 0; i<MAX_AXIS_626; i++) { homeposition[i] = 0; } // open DLL to talk to the board if (S626_DLLOpen()==0) { m_systemAvailable = true; // Declare Model 626 board to driver and launch the interrupt thread. // NOTE: we're supposing to only use one board. With two board we need // to specify the physical address for each board used. // ALSO: we're NOT using interrupts from the board. S626_OpenBoard( m_boardHandle, 0, 0, THREAD_PRIORITY_ABOVE_NORMAL ); unsigned long ErrCode = S626_GetErrors( 0 ); if (ErrCode) { m_systemReady = false; return (-1); } else { m_systemReady = true; } m_wBaseAddress = 0; encoderInit(); for (int i = 0; i<NUM_DACs_626; i++) { S626_WriteDAC(m_boardHandle, (int) i, 0); } return (0); } else { m_systemReady = false; m_systemAvailable = false; return (-1); } return (0); }
//************************************************************************* //* *************************系统初始化************************ * //************************************************************************* void vInitialize(void) { //<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<IO口初始化>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> vIOPortInit(); //<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<定时器初始化>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> vTimerInit(); //<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<光电编码器初始化>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> encoderInit(); //<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<<官方初始化>>>>>>>>>>>>>>>>>>>>>>>>>>>>>>> init_sci1( 0x00, 79 ); /* Initialization of SCI1 */ set_ccr( 0x00 ); /* Entire interrupt permission */ //<<<<<<<<<<<<<<<<<<<<<<<<<<<<<输出初始化信息>>>>>>>>>>>>>>>>>>>>>>>>>>>>> printf("\nInitialize is successful!"); }
//cannnot use the analog port or port 10 for quad encoders QuadEncoder quadEncoderInit(SensorPort topPort, SensorPort bottomPort, bool inverted) { QuadEncoder quad; //cannnot use the analog port or port 10 for quad encoders if (portIsAnalog(topPort) || portIsAnalog(bottomPort) || topPort == 10 || bottomPort == 10) { QuadEncoder *empty = NULL; print( "Cannnot use any analog port or digital port 10 for QuadEncoders."); return *empty; } portSetPinMode(topPort, INPUT); portSetPinMode(bottomPort, INPUT); quad.top = topPort; quad.bottom = bottomPort; quad.encoder_data = encoderInit(topPort, bottomPort, false); quad.inverted = inverted; return quad; }
void UserInterface::init(void) { FastLED.addLeds<NEOPIXEL, PIN_NEOPIXEL>(s_leds, NUM_LEDS); FastLED.setBrightness(s_brightness); INTR::init(); (void)NVM::init(); (void)Display::init(DISPLAY_MID_BRIGHTNESS); (void)Player::init(); encoderInit(); (void)INTR::attach(PIN_ROT_ENC_A, encoder_rotate, IRQC_CHANGE); (void)INTR::attach(PIN_ROT_ENC_B, encoder_rotate, IRQC_CHANGE); (void)INTR::attach(PIN_ROT_ENC_SW, switch_pressed, IRQC_CHANGE); (void)INTR::attach(PIN_ROT_ENC_BR_A, encoder_brightness, IRQC_CHANGE); (void)NVM::read16(NVM_COUNT_INDEX, _total_count); (void)NVM::read16(NVM_TIME_INDEX, _default_minutes); _minutes = _default_minutes; }
int main(void) { // Initialize all hardware PLL_Init(); eStopInit(); encoderInit(actlPos); motorInit(); lightsInit(); lightsUpdate(COLOR_RED); UART_Init(); Timer1_Init(); softRun(); // Send welcome message to UART terminal UART_OutChar('W');UART_OutChar('e');UART_OutChar('l');UART_OutChar('c'); UART_OutChar('o');UART_OutChar('m');UART_OutChar('e'); UART_OutChar(CR);UART_OutChar(LF); // Spin forever while(1) { parse(UART_InUDec()); // read commands from UART // All other functions performed by Timer 1 interrupt handler } }