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
}
}
