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); } }
void vTaskUltraSonic() { Ultrasonic us = ultrasonicInit(ULTRASONIC_ECHO_CHANNEL, ULTRASONIC_PING_CHANNEL); unsigned long nextWakeTime; while (1) { nextWakeTime = millis(); //get reading from sensor usValue = ultrasonicGet(us); fprintf(stdout,"Ultrasonc value = %d\n", usValue); taskDelayUntil(&nextWakeTime, 50); } }
void operatorControl() { //autonomous(); unsigned long prevWakeupTime = millis(); // PID controller variable struct pid_dat arm_pid; initPID(&arm_pid, 2, 0.5, 0.02); while (1) { /* Stores which joysticks are connected * 0 - None * 1 - Only Joystick 1 * 2 - Only Joystick 2 * 3 - Both Josticks 1 and 2 */ int joystickStatus = 0; if (isJoystickConnected(1)) joystickStatus |= 1; if (isJoystickConnected(2)) joystickStatus |= 2; // Local Variables int moveX = 0, moveY = 0, rotate = 0, arm = 0, liftSpeed = 0; int L, R, C; bool liftUp = 0, liftDw = 0, supUp = 0, supDw = 0, liftCenter = 0, liftLower = 0; // Subteam Definition int team1 = digitalRead(1); // Get Joystick Values based on Status if (joystickStatus == 3) { // Driving moveX = joystickGetAnalog(1, JOY_LX); // Movement moveY = joystickGetAnalog(1, JOY_LY); // Movement rotate = joystickGetAnalog(1, JOY_RX); // Rotate arm = joystickGetAnalog(2, JOY_RY); // Support supUp = joystickGetDigital(1, JOY_LBUM, JOY_DOWN); supDw = joystickGetDigital(1, JOY_LBUM, JOY_UP); // Both joysticks connected, reference as 1 or 2 liftUp = joystickGetDigital(1, JOY_RBUM, JOY_DOWN); liftDw = joystickGetDigital(1, JOY_RBUM, JOY_UP); liftCenter = joystickGetDigital(2, JOY_RBUM, JOY_UP); liftLower = joystickGetDigital(2, JOY_RBUM, JOY_DOWN); } else { // Driving moveX = joystickGetAnalog(joystickStatus, JOY_LX); // Movement moveY = joystickGetAnalog(joystickStatus, JOY_LY); // Movement rotate = joystickGetAnalog(joystickStatus, JOY_RX); // Rotate // Support supUp = joystickGetDigital(joystickStatus, JOY_LBUM, JOY_DOWN); supDw = joystickGetDigital(joystickStatus, JOY_LBUM, JOY_UP); // Lift liftUp = joystickGetDigital(joystickStatus, JOY_RBUM, JOY_DOWN); liftDw = joystickGetDigital(joystickStatus, JOY_RBUM, JOY_UP); } // Driving L = CAP(moveY + rotate, RANGE_MAX); R = CAP(moveY - rotate, RANGE_MAX); C = CAP(moveX, RANGE_MAX); motorSet(MC_WHEEL_L, team1 == ON ? L : L*2/3); motorSet(MC_WHEEL_R, team1 == ON ? -R : -R*2/3); motorSet(MC_WHEEL_M, team1 == ON ? -C : -C*2/3); // Support if (supDw) motorSet(MC_SUPPORT, team1 == ON ? -32 : 32); else if (supUp) motorSet(MC_SUPPORT, team1 == ON ? 32 : -32); else motorSet(MC_SUPPORT, 0); if (team1 == ON) { double loc = ((MAP_INPUT(arm) + 1.0) / 2.0) * 0.9 - 0.8; if(liftCenter) { loc = -0.35; } else if(liftLower) { loc = -0.45; } liftSpeed = -MAP_OUTPUT(computePID(loc, MAP_POT(analogRead(1)), &arm_pid )); } if (liftUp) liftSpeed = LIFTSPEED; else if (liftDw) liftSpeed = -LIFTSPEED; if(team1 != ON) { liftSpeed = -liftSpeed; } // Lift motorSet(MC_LIFT_BL, liftSpeed); motorSet(MC_LIFT_ML, liftSpeed); motorSet(MC_LIFT_TL, liftSpeed); motorSet(MC_LIFT_BR, -liftSpeed); motorSet(MC_LIFT_MR, -liftSpeed); motorSet(MC_LIFT_TR, -liftSpeed); // Motors can only be updated once every 20ms, therefore updating at twice the rate for better response time taskDelayUntil(&prevWakeupTime, 10); } }