void LiftTask(void *ignore) {
int counts;
while (1) {
imeGet(IME_LIFT, &counts); // get lift encoder count
if (joystickGetDigital(1, 8, JOY_UP) || joystickGetDigital(2, 8, JOY_UP)) { // check stash waypoint
liftPIDRequestedValue = LIFT_STASH_HEIGHT;
} else if (joystickGetDigital(1, 8, JOY_LEFT) || joystickGetDigital(2, 8, JOY_LEFT)) { // check bump waypoint
liftPIDRequestedValue = LIFT_BUMP_HEIGHT;
} else if (joystickGetDigital(1, 8, JOY_DOWN) || joystickGetDigital(2, 8, JOY_DOWN)) { // check floor waypoint
liftPIDRequestedValue = LIFT_FLOOR_HEIGHT;
} else if (joystickGetDigital(1, 8, JOY_RIGHT) || joystickGetDigital(2, 8, JOY_RIGHT)) { // check hang waypoint
liftPIDRequestedValue = LIFT_HANG_HEIGHT;
} else if (joystickGetDigital(1, 6, JOY_UP) || joystickGetDigital(2, 6, JOY_UP)) { // if trigger up, turn PID off and override motors up, then turn PID on for the new position
mutexTake(liftMutex, ULONG_MAX);
liftSet(127);
liftPIDRequestedValue = counts;
mutexGive(liftMutex);
} else if (joystickGetDigital(1, 6, JOY_DOWN) || joystickGetDigital(2, 6, JOY_DOWN)) { // if trigger up, turn PID off and override motors up, then turn PID on for the new position
mutexTake(liftMutex, ULONG_MAX);
liftSet(-127);
liftPIDRequestedValue = counts;
mutexGive(liftMutex);
} else {
liftSet(0);
}
taskDelay(20);
}
}
void vTaskPIController() {
//initialize values
mutexTake(piSetpointMutex, -1);
leftMotor.position=0; leftMotor.setpoint=0; leftMotor.output = 0; leftMotor.i_accum = 0;
rightMotor.position=0; rightMotor.setpoint=0; rightMotor.output = 0; rightMotor.i_accum = 0;
mutexGive(piSetpointMutex);
unsigned long taskWakeTime;
//update PI control every 25ms
while(1) {
taskWakeTime = millis();
// take the mutex
mutexTake(piSetpointMutex, -1);
// update odometry
imeGet(LEFT_IME_ADDR, &(leftMotor.position));
imeGet(RIGHT_IME_ADDR, &(rightMotor.position));
// compute control laws
pi_update(&leftMotor);
pi_update(&rightMotor);
// set motor outputs
motorSet(LEFT_MOTOR_CHANNEL, leftMotor.output);
motorSet(RIGHT_MOTOR_CHANNEL, rightMotor.output);
// release mutex
mutexGive(piSetpointMutex);
// sleep for 25 milliseconds
taskDelayUntil(&taskWakeTime, 25);
}
}
int32 EventLoop::addAsynOperator(NetOperator* op) {
int32 ret = 0;
if(op) {
mutexTake(&m_opcs);
m_ops.autoResizeWrite((const uint8*)&op, sizeof(op));
mutexGive(&m_opcs);
}
int32 flag = atomicCompareExchange32(&m_ops_flag, 1, 0);
if(!flag) {
char ctl;
ret = send(m_ctlfd1, &ctl, sizeof(ctl), 0);
if(ret < 0) {
NLogError << "EventLoop:" << std::hex
<< this << ", send ctl msg fail";
return -1;
} else {
#if DETAIL_NET_LOG
NLogTrace << "EventLoop:" << std::hex
<< this << ", send ctl msg";
#endif
}
}
return ret;
}
int32 EventLoop::processOps() {
NetOperator *op = NULL;
char buf[1024 * 16];
int32 loop = sizeof(buf);
atomicExchange32(&m_ops_flag, 0);
loop = recv(m_ctlfd, buf, sizeof(buf), 0);
int32 cnt = 0;
while(m_ops.size()) {
mutexTake(&m_opcs);
if(m_ops.size() >= (int32)sizeof(op)) {
m_ops.read((uint8*)&op, sizeof(op));
mutexGive(&m_opcs);
} else {
mutexGive(&m_opcs);
break;
}
//recv notify
assert(op);
op->process(this);
delete op;
++cnt;
}
return 0;
}
// Sets target RPM.
void flywheelSet(Flywheel *flywheel, float rpm)
{
mutexTake(flywheel->targetMutex, -1); // TODO: figure out how long the block time should be.
flywheel->target = rpm;
mutexGive(flywheel->targetMutex);
if (flywheel->ready)
{
activate(flywheel);
}
//printf("Debug Target set to %f rpm.\n", rpm);
}
/**
* @brief Handle interrupt from mailbox
* @return Nothing
*/
void MAILBOX_IRQHandler(void)
{
/* Slave core uses passed mailbox value as address to shared LED state values */
uint32_t *psharedLEDStates = (uint32_t *) Chip_MBOX_GetValue(LPC_MBOX, myCoreBox);
/* Toggle LED bit state for this core usign mutex */
mutexTake();
sharedLEDStates = *psharedLEDStates;
ledToggleBit(1);
*psharedLEDStates = sharedLEDStates;
mutexGive();
/* Clear this MCU's mailbox */
Chip_MBOX_ClearValueBits(LPC_MBOX, myCoreBox, 0xFFFFFFFF);
/* Signal master code about the change */
Chip_MBOX_SetValue(LPC_MBOX, otherCoreBox, 1);
}
int32 EventLoop::delAllOp() {
NetOperator *op = NULL;
while(1) {
mutexTake(&m_opcs);
if(m_ops.size() >= (int32)sizeof(op)) {
m_ops.read((uint8*)&op, sizeof(op));
mutexGive(&m_opcs);
} else {
mutexGive(&m_opcs);
break;
}
//recv notify
assert(op);
delete op;
}
return 0;
}
void LiftPIDTask(void *ignore) {
int counts;
while (1) {
imeGet(IME_LIFT, &counts); // block until lift resources are available
if (mutexTake(liftMutex, ULONG_MAX)) { // block until mutex is available.
pidSensorCurrentValue = counts; // getting current position
pidError = pidSensorCurrentValue - liftPIDRequestedValue; // calculating error signal
if (abs(pidError) < PID_INTEGRAL_LIMIT) { // calculating integral factor, given it is within bounds
pidIntegral = pidIntegral + pidError;
} else {
pidIntegral = 0;
}
pidDerivative = pidError - pidLastError; // calculate derivative factor
pidLastError = pidError;
pidDrive = (pid_Kp * pidError) + (pid_Ki * pidIntegral) + (pid_Kd * pidDerivative); // sum all factors
if (pidDrive > PID_DRIVE_MAX) { // limit max output
pidDrive = PID_DRIVE_MAX;
}
if (pidDrive < PID_DRIVE_MIN) {
pidDrive = PID_DRIVE_MIN;
}
liftSet (pidDrive * PID_MOTOR_SCALE); // set lift motors
mutexGive(liftMutex); // give control back to LiftTask.
} else { // reset all
pidError = 0;
pidLastError = 0;
pidIntegral = 0;
pidDerivative = 0;
}
taskDelay(25);
}
}
void measureRpm(Flywheel *flywheel, float timeChange)
{
int reading = encoderGet(flywheel->encoder);
int ticks = reading - flywheel->reading;
// Raw rpm
float rpm = ticks / flywheel->encoderTicksPerRevolution * flywheel->gearing / timeChange * 60;
// Low-pass filter
float measureChange = (rpm - flywheel->measured) * timeChange / flywheel->smoothing;
// Update
flywheel->reading = reading;
flywheel->measuredRaw = rpm;
flywheel->measured += measureChange;
flywheel->derivative = measureChange / timeChange;
// Calculate error
mutexTake(flywheel->targetMutex, -1); // TODO: Find out what block time is suitable, or needeed at all.
flywheel->error = flywheel->measured - flywheel->target;
mutexGive(flywheel->targetMutex);
}
/*
* Runs the user operator control code. This function will be started in its own task with the
* default priority and stack size whenever the robot is enabled via the Field Management System
* or the VEX Competition Switch in the operator control mode. If the robot is disabled or
* communications is lost, the operator control task will be stopped by the kernel. Re-enabling
* the robot will restart the task, not resume it from where it left off.
*
* If no VEX Competition Switch or Field Management system is plugged in, the VEX Cortex will
* run the operator control task. Be warned that this will also occur if the VEX Cortex is
* tethered directly to a computer via the USB A to A cable without any VEX Joystick attached.
*
* Code running in this task can take almost any action, as the VEX Joystick is available and
* the scheduler is operational. However, proper use of delay() or taskDelayUntil() is highly
* recommended to give other tasks (including system tasks such as updating LCDs) time to run.
*
* This task should never exit; it should end with some kind of infinite loop, even if empty.
*/
void operatorControl() {
// start IR detection task
taskCreate(vTaskFilter, 2048, NULL, TASK_PRIORITY_DEFAULT);
// start PI controller
taskCreate(vTaskPIController, TASK_DEFAULT_STACK_SIZE, NULL, TASK_PRIORITY_DEFAULT);
//start polling the UltraSonic sensor
taskCreate(vTaskUltraSonic, TASK_DEFAULT_STACK_SIZE, NULL, TASK_PRIORITY_DEFAULT);
// show the detected IR levels
/*short leftIR_l, centerIR_l, rightIR_l;
while(1) {
if(mutexTake(irDetectorMutex, 10)) {
leftIR_l = leftIR;
centerIR_l = centerIR;
rightIR_l = rightIR;
mutexGive(irDetectorMutex);
}
printf("Left: %d Center: %d Right: %d \n", leftIR_l, centerIR_l, rightIR_l);
taskDelay(100);
}*/
// advance 1000, reverse 1000, every 2 seconds
int multiplier = 1;
while(1) {
// take PI Mutex
mutexTake(piSetpointMutex, -1);
leftMotor.position += 1000*multiplier;
rightMotor.position += 1000*multiplier;
mutexGive(piSetpointMutex);
multiplier = -1*multiplier;
taskDelay(2000);
}
}