void Scenes_MoveToZeroPosition(void) {
Servo_SetPositionsTorque(0); // Maximum torque
for (unsigned char i = 0; i < sizeof(wCK_IDs); i++) {
Servo_SetPosition(i, (float)(MotionZeroPos[i] - 127) * 1.055);
}
Servo_SendPositions();
}
task Hopper()
{
Joystick_WaitForStart();
while (true) {
//if (servo_turntable_pos<pos_servo_turntable_BL) {
// servo_turntable_pos = pos_servo_turntable_BL;
//} else if (servo_turntable_pos>pos_servo_turntable_BR) {
// servo_turntable_pos = pos_servo_turntable_BR;
//}
if (lift_pos<pos_hopper_safety_down) {
servo_turntable_pos = pos_servo_turntable_F;
}
if (hopper_pos<pos_hopper_safety_side) {
servo_turntable_pos = pos_servo_turntable_F;
}
servo_turntable_pos = pos_servo_turntable_F;
Servo_SetPosition(servo_turntable, (int)round(servo_turntable_pos));
if (lift_pos < pos_hopper_safety_down) {
if (hopper_pos > (pos_servo_hopper_down+3)) {
int lift_target_buf = lift_target;
int hopper_target_buf = hopper_target;
while (hopper_pos > (pos_servo_hopper_down+3)) {
lift_target = pos_hopper_safety_down;
is_lift_manual = false;
hopper_pos = encoderToHopper(encoder_hopper);
hopper_target = pos_servo_hopper_down;
Servo_SetPosition(servo_hopper_A, hopper_target);
Servo_SetPosition(servo_hopper_B, hopper_target);
Time_Wait(5);
}
lift_target = lift_target_buf;
hopper_target = hopper_target_buf;
}
}
if (hopper_target < pos_servo_hopper_down) {
hopper_target = pos_servo_hopper_down;
} else if (hopper_target > pos_servo_hopper_goal) {
hopper_target = pos_servo_hopper_goal;
}
Servo_SetPosition(servo_hopper_A, hopper_target);
Servo_SetPosition(servo_hopper_B, hopper_target);
Time_Wait(2);
}
}
// Set the index LED, property with the value
int ServoOsc_PropertySet( int index, int property, int value )
{
switch ( property )
{
case 0:
Servo_SetActive( index, value );
break;
case 1:
Servo_SetPosition( index, value );
break;
case 2:
Servo_SetSpeed( index, value );
break;
}
return CONTROLLER_OK;
}
void Scenes_SendFrame(struct TScene* scene, int frameIndex, float* interpolationDistanceArray) {
int lwtmp;
int i;
Servo_SetPositionsTorque(scene->wCK[0].Torq);
for (i = 0; i < MAX_wCK; i++) { // for all wCK
if (scene->wCK[i].Exist) { // if wCK exists add the interpolation step
lwtmp = (int)scene->wCK[i].SPos + (int)((float)frameIndex * interpolationDistanceArray[i]);
if (lwtmp > 254) { // bound result 1 to 254
lwtmp = 254;
} else if (lwtmp < 1) {
lwtmp = 1;
}
Servo_SetPosition(i, (float)(lwtmp - 127) * 1.055);
}
}
Servo_SendPositions();
}
void Walker_ResetArmPositions(void)
{
#ifdef DYNAMIXEL
Servo_SetPosition(SERVO_ID_R_SHOULDER_PITCH, 48.345);
Servo_SetPosition(SERVO_ID_L_SHOULDER_PITCH, -41.313);
Servo_SetPosition(SERVO_ID_R_SHOULDER_ROLL, 17.873);
Servo_SetPosition(SERVO_ID_L_SHOULDER_ROLL, -17.580);
Servo_SetPosition(SERVO_ID_R_ELBOW, -29.300);
Servo_SetPosition(SERVO_ID_L_ELBOW, 29.593);
#else
inverse_startCycle();
struct vect3d_ext target;
target.x = 0;
target.y = 0;
target.z = 80;
target.yaw = 0;
inverse_calc(ARM_LEFT, &target);
inverse_calc(ARM_RIGHT, &target);
inverse_endCycle(0);
#endif
}
void Walker_Process(void) {
float ep[12];
float angle[14];
float outValue[14];
float x_swap, y_swap, z_swap;
float x_move_r, y_move_r, z_move_r, c_move_r;
float x_move_l, y_move_l, z_move_l, c_move_l;
float pelvis_offset_r, pelvis_offset_l;
float dt = TC_GetMsSinceTick(walker_lastUpdate);
walker_lastUpdate = TC_GetSystemTicks();
// Update walk parameters
if (m_Time == 0) {
Walker_update_param_time();
m_Phase = PHASE0;
if (m_Ctrl_Running == 0) {
if (m_X_Move_Amplitude == 0 && m_Y_Move_Amplitude == 0 && m_A_Move_Amplitude == 0) {
m_Real_Running = 0;
} else {
Walker_X_MOVE_AMPLITUDE = 0;
Walker_Y_MOVE_AMPLITUDE = 0;
Walker_A_MOVE_AMPLITUDE = 0;
}
}
} else if (m_Time >= (m_Phase_Time1 - dt / 2) && m_Time < (m_Phase_Time1 + dt / 2)) {
Walker_update_param_move();
m_Phase = PHASE1;
} else if (m_Time >= (m_Phase_Time2 - dt / 2) && m_Time < (m_Phase_Time2 + dt / 2)) {
Walker_update_param_time();
m_Time = m_Phase_Time2;
m_Phase = PHASE2;
if (m_Ctrl_Running == 0) {
if (m_X_Move_Amplitude == 0 && m_Y_Move_Amplitude == 0 && m_A_Move_Amplitude == 0) {
m_Real_Running = 0;
} else {
Walker_X_MOVE_AMPLITUDE = 0;
Walker_Y_MOVE_AMPLITUDE = 0;
Walker_A_MOVE_AMPLITUDE = 0;
}
}
} else if (m_Time >= (m_Phase_Time3 - dt / 2) && m_Time < (m_Phase_Time3 + dt / 2)) {
Walker_update_param_move();
m_Phase = PHASE3;
}
Walker_update_param_balance();
// Compute endpoints
x_swap = wsin(m_Time, m_X_Swap_PeriodTime, m_X_Swap_Phase_Shift, m_X_Swap_Amplitude, m_X_Swap_Amplitude_Shift);
y_swap = wsin(m_Time, m_Y_Swap_PeriodTime, m_Y_Swap_Phase_Shift, m_Y_Swap_Amplitude, m_Y_Swap_Amplitude_Shift);
z_swap = wsin(m_Time, m_Z_Swap_PeriodTime, m_Z_Swap_Phase_Shift, m_Z_Swap_Amplitude, m_Z_Swap_Amplitude_Shift);
if(m_Time <= m_SSP_Time_Start_L) {
x_move_l = wsin(m_SSP_Time_Start_L, m_X_Move_PeriodTime, m_X_Move_Phase_Shift + 2 * M_PI / m_X_Move_PeriodTime * m_SSP_Time_Start_L, m_X_Move_Amplitude, m_X_Move_Amplitude_Shift);
y_move_l = wsin(m_SSP_Time_Start_L, m_Y_Move_PeriodTime, m_Y_Move_Phase_Shift + 2 * M_PI / m_Y_Move_PeriodTime * m_SSP_Time_Start_L, m_Y_Move_Amplitude, m_Y_Move_Amplitude_Shift);
z_move_l = wsin(m_SSP_Time_Start_L, m_Z_Move_PeriodTime, m_Z_Move_Phase_Shift + 2 * M_PI / m_Z_Move_PeriodTime * m_SSP_Time_Start_L, m_Z_Move_Amplitude, m_Z_Move_Amplitude_Shift);
c_move_l = wsin(m_SSP_Time_Start_L, m_A_Move_PeriodTime, m_A_Move_Phase_Shift + 2 * M_PI / m_A_Move_PeriodTime * m_SSP_Time_Start_L, m_A_Move_Amplitude, m_A_Move_Amplitude_Shift);
x_move_r = wsin(m_SSP_Time_Start_L, m_X_Move_PeriodTime, m_X_Move_Phase_Shift + 2 * M_PI / m_X_Move_PeriodTime * m_SSP_Time_Start_L, -m_X_Move_Amplitude, -m_X_Move_Amplitude_Shift);
y_move_r = wsin(m_SSP_Time_Start_L, m_Y_Move_PeriodTime, m_Y_Move_Phase_Shift + 2 * M_PI / m_Y_Move_PeriodTime * m_SSP_Time_Start_L, -m_Y_Move_Amplitude, -m_Y_Move_Amplitude_Shift);
z_move_r = wsin(m_SSP_Time_Start_R, m_Z_Move_PeriodTime, m_Z_Move_Phase_Shift + 2 * M_PI / m_Z_Move_PeriodTime * m_SSP_Time_Start_R, m_Z_Move_Amplitude, m_Z_Move_Amplitude_Shift);
c_move_r = wsin(m_SSP_Time_Start_L, m_A_Move_PeriodTime, m_A_Move_Phase_Shift + 2 * M_PI / m_A_Move_PeriodTime * m_SSP_Time_Start_L, -m_A_Move_Amplitude, -m_A_Move_Amplitude_Shift);
pelvis_offset_l = 0;
pelvis_offset_r = 0;
} else if(m_Time <= m_SSP_Time_End_L) {
x_move_l = wsin(m_Time, m_X_Move_PeriodTime, m_X_Move_Phase_Shift + 2 * M_PI / m_X_Move_PeriodTime * m_SSP_Time_Start_L, m_X_Move_Amplitude, m_X_Move_Amplitude_Shift);
y_move_l = wsin(m_Time, m_Y_Move_PeriodTime, m_Y_Move_Phase_Shift + 2 * M_PI / m_Y_Move_PeriodTime * m_SSP_Time_Start_L, m_Y_Move_Amplitude, m_Y_Move_Amplitude_Shift);
z_move_l = wsin(m_Time, m_Z_Move_PeriodTime, m_Z_Move_Phase_Shift + 2 * M_PI / m_Z_Move_PeriodTime * m_SSP_Time_Start_L, m_Z_Move_Amplitude, m_Z_Move_Amplitude_Shift);
c_move_l = wsin(m_Time, m_A_Move_PeriodTime, m_A_Move_Phase_Shift + 2 * M_PI / m_A_Move_PeriodTime * m_SSP_Time_Start_L, m_A_Move_Amplitude, m_A_Move_Amplitude_Shift);
x_move_r = wsin(m_Time, m_X_Move_PeriodTime, m_X_Move_Phase_Shift + 2 * M_PI / m_X_Move_PeriodTime * m_SSP_Time_Start_L, -m_X_Move_Amplitude, -m_X_Move_Amplitude_Shift);
y_move_r = wsin(m_Time, m_Y_Move_PeriodTime, m_Y_Move_Phase_Shift + 2 * M_PI / m_Y_Move_PeriodTime * m_SSP_Time_Start_L, -m_Y_Move_Amplitude, -m_Y_Move_Amplitude_Shift);
z_move_r = wsin(m_SSP_Time_Start_R, m_Z_Move_PeriodTime, m_Z_Move_Phase_Shift + 2 * M_PI / m_Z_Move_PeriodTime * m_SSP_Time_Start_R, m_Z_Move_Amplitude, m_Z_Move_Amplitude_Shift);
c_move_r = wsin(m_Time, m_A_Move_PeriodTime, m_A_Move_Phase_Shift + 2 * M_PI / m_A_Move_PeriodTime * m_SSP_Time_Start_L, -m_A_Move_Amplitude, -m_A_Move_Amplitude_Shift);
pelvis_offset_l = wsin(m_Time, m_Z_Move_PeriodTime, m_Z_Move_Phase_Shift + 2 * M_PI / m_Z_Move_PeriodTime * m_SSP_Time_Start_L, m_Pelvis_Swing / 2, m_Pelvis_Swing / 2);
pelvis_offset_r = wsin(m_Time, m_Z_Move_PeriodTime, m_Z_Move_Phase_Shift + 2 * M_PI / m_Z_Move_PeriodTime * m_SSP_Time_Start_L, -m_Pelvis_Offset / 2, -m_Pelvis_Offset / 2);
} else if(m_Time <= m_SSP_Time_Start_R) {
x_move_l = wsin(m_SSP_Time_End_L, m_X_Move_PeriodTime, m_X_Move_Phase_Shift + 2 * M_PI / m_X_Move_PeriodTime * m_SSP_Time_Start_L, m_X_Move_Amplitude, m_X_Move_Amplitude_Shift);
y_move_l = wsin(m_SSP_Time_End_L, m_Y_Move_PeriodTime, m_Y_Move_Phase_Shift + 2 * M_PI / m_Y_Move_PeriodTime * m_SSP_Time_Start_L, m_Y_Move_Amplitude, m_Y_Move_Amplitude_Shift);
z_move_l = wsin(m_SSP_Time_End_L, m_Z_Move_PeriodTime, m_Z_Move_Phase_Shift + 2 * M_PI / m_Z_Move_PeriodTime * m_SSP_Time_Start_L, m_Z_Move_Amplitude, m_Z_Move_Amplitude_Shift);
c_move_l = wsin(m_SSP_Time_End_L, m_A_Move_PeriodTime, m_A_Move_Phase_Shift + 2 * M_PI / m_A_Move_PeriodTime * m_SSP_Time_Start_L, m_A_Move_Amplitude, m_A_Move_Amplitude_Shift);
x_move_r = wsin(m_SSP_Time_End_L, m_X_Move_PeriodTime, m_X_Move_Phase_Shift + 2 * M_PI / m_X_Move_PeriodTime * m_SSP_Time_Start_L, -m_X_Move_Amplitude, -m_X_Move_Amplitude_Shift);
y_move_r = wsin(m_SSP_Time_End_L, m_Y_Move_PeriodTime, m_Y_Move_Phase_Shift + 2 * M_PI / m_Y_Move_PeriodTime * m_SSP_Time_Start_L, -m_Y_Move_Amplitude, -m_Y_Move_Amplitude_Shift);
z_move_r = wsin(m_SSP_Time_Start_R, m_Z_Move_PeriodTime, m_Z_Move_Phase_Shift + 2 * M_PI / m_Z_Move_PeriodTime * m_SSP_Time_Start_R, m_Z_Move_Amplitude, m_Z_Move_Amplitude_Shift);
c_move_r = wsin(m_SSP_Time_End_L, m_A_Move_PeriodTime, m_A_Move_Phase_Shift + 2 * M_PI / m_A_Move_PeriodTime * m_SSP_Time_Start_L, -m_A_Move_Amplitude, -m_A_Move_Amplitude_Shift);
pelvis_offset_l = 0;
pelvis_offset_r = 0;
} else if(m_Time <= m_SSP_Time_End_R) {
x_move_l = wsin(m_Time, m_X_Move_PeriodTime, m_X_Move_Phase_Shift + 2 * M_PI / m_X_Move_PeriodTime * m_SSP_Time_Start_R + M_PI, m_X_Move_Amplitude, m_X_Move_Amplitude_Shift);
y_move_l = wsin(m_Time, m_Y_Move_PeriodTime, m_Y_Move_Phase_Shift + 2 * M_PI / m_Y_Move_PeriodTime * m_SSP_Time_Start_R + M_PI, m_Y_Move_Amplitude, m_Y_Move_Amplitude_Shift);
z_move_l = wsin(m_SSP_Time_End_L, m_Z_Move_PeriodTime, m_Z_Move_Phase_Shift + 2 * M_PI / m_Z_Move_PeriodTime * m_SSP_Time_Start_L, m_Z_Move_Amplitude, m_Z_Move_Amplitude_Shift);
c_move_l = wsin(m_Time, m_A_Move_PeriodTime, m_A_Move_Phase_Shift + 2 * M_PI / m_A_Move_PeriodTime * m_SSP_Time_Start_R + M_PI, m_A_Move_Amplitude, m_A_Move_Amplitude_Shift);
x_move_r = wsin(m_Time, m_X_Move_PeriodTime, m_X_Move_Phase_Shift + 2 * M_PI / m_X_Move_PeriodTime * m_SSP_Time_Start_R + M_PI, -m_X_Move_Amplitude, -m_X_Move_Amplitude_Shift);
y_move_r = wsin(m_Time, m_Y_Move_PeriodTime, m_Y_Move_Phase_Shift + 2 * M_PI / m_Y_Move_PeriodTime * m_SSP_Time_Start_R + M_PI, -m_Y_Move_Amplitude, -m_Y_Move_Amplitude_Shift);
z_move_r = wsin(m_Time, m_Z_Move_PeriodTime, m_Z_Move_Phase_Shift + 2 * M_PI / m_Z_Move_PeriodTime * m_SSP_Time_Start_R, m_Z_Move_Amplitude, m_Z_Move_Amplitude_Shift);
c_move_r = wsin(m_Time, m_A_Move_PeriodTime, m_A_Move_Phase_Shift + 2 * M_PI / m_A_Move_PeriodTime * m_SSP_Time_Start_R + M_PI, -m_A_Move_Amplitude, -m_A_Move_Amplitude_Shift);
pelvis_offset_l = wsin(m_Time, m_Z_Move_PeriodTime, m_Z_Move_Phase_Shift + 2 * M_PI / m_Z_Move_PeriodTime * m_SSP_Time_Start_R, m_Pelvis_Offset / 2, m_Pelvis_Offset / 2);
pelvis_offset_r = wsin(m_Time, m_Z_Move_PeriodTime, m_Z_Move_Phase_Shift + 2 * M_PI / m_Z_Move_PeriodTime * m_SSP_Time_Start_R, -m_Pelvis_Swing / 2, -m_Pelvis_Swing / 2);
} else {
x_move_l = wsin(m_SSP_Time_End_R, m_X_Move_PeriodTime, m_X_Move_Phase_Shift + 2 * M_PI / m_X_Move_PeriodTime * m_SSP_Time_Start_R + M_PI, m_X_Move_Amplitude, m_X_Move_Amplitude_Shift);
y_move_l = wsin(m_SSP_Time_End_R, m_Y_Move_PeriodTime, m_Y_Move_Phase_Shift + 2 * M_PI / m_Y_Move_PeriodTime * m_SSP_Time_Start_R + M_PI, m_Y_Move_Amplitude, m_Y_Move_Amplitude_Shift);
z_move_l = wsin(m_SSP_Time_End_L, m_Z_Move_PeriodTime, m_Z_Move_Phase_Shift + 2 * M_PI / m_Z_Move_PeriodTime * m_SSP_Time_Start_L, m_Z_Move_Amplitude, m_Z_Move_Amplitude_Shift);
c_move_l = wsin(m_SSP_Time_End_R, m_A_Move_PeriodTime, m_A_Move_Phase_Shift + 2 * M_PI / m_A_Move_PeriodTime * m_SSP_Time_Start_R + M_PI, m_A_Move_Amplitude, m_A_Move_Amplitude_Shift);
x_move_r = wsin(m_SSP_Time_End_R, m_X_Move_PeriodTime, m_X_Move_Phase_Shift + 2 * M_PI / m_X_Move_PeriodTime * m_SSP_Time_Start_R + M_PI, -m_X_Move_Amplitude, -m_X_Move_Amplitude_Shift);
y_move_r = wsin(m_SSP_Time_End_R, m_Y_Move_PeriodTime, m_Y_Move_Phase_Shift + 2 * M_PI / m_Y_Move_PeriodTime * m_SSP_Time_Start_R + M_PI, -m_Y_Move_Amplitude, -m_Y_Move_Amplitude_Shift);
z_move_r = wsin(m_SSP_Time_End_R, m_Z_Move_PeriodTime, m_Z_Move_Phase_Shift + 2 * M_PI / m_Z_Move_PeriodTime * m_SSP_Time_Start_R, m_Z_Move_Amplitude, m_Z_Move_Amplitude_Shift);
c_move_r = wsin(m_SSP_Time_End_R, m_A_Move_PeriodTime, m_A_Move_Phase_Shift + 2 * M_PI / m_A_Move_PeriodTime * m_SSP_Time_Start_R + M_PI, -m_A_Move_Amplitude, -m_A_Move_Amplitude_Shift);
pelvis_offset_l = 0;
pelvis_offset_r = 0;
}
ep[0] = x_swap + x_move_r + m_X_Offset;
ep[1] = y_swap + y_move_r - m_Y_Offset / 2;
ep[2] = z_swap + z_move_r + m_Z_Offset; // z default: 23
ep[3] = 0 + 0 - m_R_Offset / 2;
ep[4] = 0 + 0 + m_P_Offset;
ep[5] = c_move_r - m_A_Offset / 2;
ep[6] = x_swap + x_move_l + m_X_Offset;
ep[7] = y_swap + y_move_l + m_Y_Offset / 2;
ep[8] = z_swap + z_move_l + m_Z_Offset; // z default: 23
ep[9] = 0 + 0 + m_R_Offset / 2;
ep[10] = 0 + 0 + m_P_Offset;
ep[11] = c_move_l + m_A_Offset / 2;
// Compute arm swing
if(m_X_Move_Amplitude == 0) {
angle[12] = 0; // Right
angle[13] = 0; // Left
} else {
angle[12] = wsin(m_Time, m_PeriodTime, M_PI * 1.5, -m_X_Move_Amplitude * m_Arm_Swing_Gain, 0);
angle[13] = wsin(m_Time, m_PeriodTime, M_PI * 1.5, m_X_Move_Amplitude * m_Arm_Swing_Gain, 0);
}
if(m_Real_Running == 1) {
m_Time += dt;
if(m_Time >= m_PeriodTime) {
m_Time = 0;
odometry_calculateWalkerPos(m_X_Move_Amplitude,m_Y_Move_Amplitude,Walker_A_MOVE_AMPLITUDE);
}
}
// Compute angles
//dbgu_printf("Leg 1: x=%3.6f y=%3.6f z=%3.6f yaw=%3.6f\r\n", ep[0], ep[1], ep[2], ep[5]);
//dbgu_printf("Leg 2: x=%3.6f y=%3.6f z=%3.6f yaw=%3.6f\r\n", ep[6], ep[7], ep[8], ep[11]);
if ((Walker_computeIK_Wolves(&angle[0], ep[0], ep[1], ep[2], ep[3], ep[4], ep[5]) == 1)
&& (Walker_computeIK_Wolves(&angle[6], ep[6], ep[7], ep[8], ep[9], ep[10], ep[11]) == 1)) {
for (int i = 0; i < 12; i++) {
angle[i] *= 180.0 / M_PI;
}
} else {
return; // Do not use angle;
}
// Compute motor value
for (int i = 0; i < 14; i++) {
float offset = (double)dir[i] * angle[i];
if (i == 1) { // R_HIP_ROLL
offset += (double)dir[i] * pelvis_offset_r;
} else if (i == 7) { // L_HIP_ROLL
offset += (double)dir[i] * pelvis_offset_l;
} else if (i == 2 || i == 8) { // R_HIP_PITCH or L_HIP_PITCH
offset -= (double)dir[i] * Walker_Params.HIP_PITCH_OFFSET;
}
outValue[i] = initAngle[i] + offset;
}
// adjust balance offset
if (Walker_Params.BALANCE_ENABLE) {
#define GYRO_TO_SERVO_RATIO 16
float rlGyroErr = (float)(IMU_GetRLGyro() / GYRO_TO_SERVO_RATIO);
float fbGyroErr = (float)(IMU_GetFBGyro() / GYRO_TO_SERVO_RATIO);
outValue[1] += (int)(dir[1] * rlGyroErr * Walker_Params.BALANCE_HIP_ROLL_GAIN); // R_HIP_ROLL
outValue[7] += (int)(dir[7] * rlGyroErr * Walker_Params.BALANCE_HIP_ROLL_GAIN); // L_HIP_ROLL
outValue[3] -= (int)(dir[3] * fbGyroErr * Walker_Params.BALANCE_KNEE_GAIN); // R_KNEE
outValue[9] -= (int)(dir[9] * fbGyroErr * Walker_Params.BALANCE_KNEE_GAIN); // L_KNEE
outValue[4] -= (int)(dir[4] * fbGyroErr * Walker_Params.BALANCE_ANKLE_PITCH_GAIN); // R_ANKLE_PITCH
outValue[10] -= (int)(dir[10] * fbGyroErr * Walker_Params.BALANCE_ANKLE_PITCH_GAIN); // L_ANKLE_PITCH
outValue[5] -= (int)(dir[5] * rlGyroErr * Walker_Params.BALANCE_ANKLE_ROLL_GAIN); // R_ANKLE_ROLL
outValue[11] -= (int)(dir[11] * rlGyroErr * Walker_Params.BALANCE_ANKLE_ROLL_GAIN); // L_ANKLE_ROLL
}
if(m_Servos_Active == 1) {
Servo_SetPosition(SERVO_ID_R_HIP_YAW, outValue[0]);
Servo_SetPosition(SERVO_ID_R_HIP_ROLL, outValue[1]);
Servo_SetPosition(SERVO_ID_R_HIP_PITCH, outValue[2]);
Servo_SetPosition(SERVO_ID_R_KNEE, outValue[3]);
Servo_SetPosition(SERVO_ID_R_ANKLE_PITCH, outValue[4]);
Servo_SetPosition(SERVO_ID_R_ANKLE_ROLL, outValue[5]);
Servo_SetPosition(SERVO_ID_L_HIP_YAW, outValue[6]);
Servo_SetPosition(SERVO_ID_L_HIP_ROLL, outValue[7]);
Servo_SetPosition(SERVO_ID_L_HIP_PITCH, outValue[8]);
Servo_SetPosition(SERVO_ID_L_KNEE, outValue[9]);
Servo_SetPosition(SERVO_ID_L_ANKLE_PITCH, outValue[10]);
Servo_SetPosition(SERVO_ID_L_ANKLE_ROLL, outValue[11]);
Servo_SetPosition(SERVO_ID_R_SHOULDER_PITCH, outValue[12]);
Servo_SetPosition(SERVO_ID_L_SHOULDER_PITCH, outValue[13]);
}
}
task main()
{
typedef enum MotorDirection {
DIRECTION_NONE = 0,
DIRECTION_IN,
DIRECTION_OUT
};
typedef enum PickupPos {
PICKUP_RETRACT = 0,
PICKUP_LARGE,
PICKUP_SMALL,
PICKUP_KICK
};
initializeGlobalVariables();
initializeRobotVariables();
MotorDirection pickup_I_direction = DIRECTION_NONE;
MotorDirection pickup_I_direction_prev = DIRECTION_NONE;
MotorDirection pickup_O_direction = DIRECTION_NONE;
MotorDirection pickup_O_direction_prev = DIRECTION_NONE;
MotorDirection clamp_direction = DIRECTION_NONE;
PickupPos pickup_pos = PICKUP_LARGE;
int servo_dump_pos = pos_servo_dump_closed;
float power_L = 0.0;
float power_R = 0.0;
float power_pickup_I = 0.0;
float power_pickup_O = 0.0;
float power_clamp = 0.0;
Task_Spawn(Gyro);
Task_Spawn(PID);
Task_Spawn(Display);
Task_Spawn(Hopper);
Joystick_WaitForStart();
while (true) {
Joystick_UpdateData();
HTIRS2readAllACStrength(sensor_IR, IR_A, IR_B, IR_C, IR_D, IR_E);
hopper_pos = encoderToHopper(Motor_GetEncoder(encoder_hopper));
power_L = Joystick_GenericInput(JOYSTICK_L, AXIS_Y);
power_R = Joystick_GenericInput(JOYSTICK_R, AXIS_Y);
power_lift_temp = Joystick_GenericInput(JOYSTICK_L, AXIS_Y, CONTROLLER_2);
if (abs(power_lift_temp) > 5) {
is_lift_manual = true;
} else {
is_lift_manual = false;
}
if (Joystick_Button(BUTTON_JOYL, CONTROLLER_2) == true) {
isReset = true;
} else {
isReset = false;
}
//servo_turntable_pos -= 0.004 * Joystick_GenericInput(JOYSTICK_R, AXIS_X, CONTROLLER_2);
if (Joystick_Button(BUTTON_JOYR, CONTROLLER_2) || Joystick_ButtonReleased(BUTTON_JOYR, CONTROLLER_2)) {
servo_turntable_pos = pos_servo_turntable_F;
hopper_target = pos_servo_hopper_down;
}
if (abs(Joystick_GenericInput(JOYSTICK_R, AXIS_Y, CONTROLLER_2))>0) {
hopper_target = hopper_pos;
hopper_target += 0.6 * Joystick_GenericInput(JOYSTICK_R, AXIS_Y, CONTROLLER_2);
}
hopper_r = sqrt(hopper_x_target*hopper_x_target + hopper_y_target*hopper_y_target);
hopper_theta = atan2(hopper_y_target, hopper_x_target);
if (Joystick_ButtonPressed(BUTTON_B)) {
pickup_I_direction_prev = pickup_I_direction;
pickup_I_direction = DIRECTION_OUT;
}
if (Joystick_ButtonPressed(BUTTON_A)) {
pickup_I_direction_prev = pickup_I_direction;
pickup_I_direction = DIRECTION_IN;
}
if (Joystick_ButtonReleased(BUTTON_B)) {
pickup_I_direction = pickup_I_direction_prev;
}
if (Joystick_ButtonReleased(BUTTON_A)) {
pickup_I_direction = pickup_I_direction_prev;
}
if (Joystick_ButtonPressed(BUTTON_Y)) {
pickup_O_direction_prev = pickup_O_direction;
pickup_O_direction = DIRECTION_OUT;
}
if (Joystick_ButtonPressed(BUTTON_X)) {
pickup_O_direction_prev = pickup_O_direction;
pickup_O_direction = DIRECTION_IN;
}
if (Joystick_ButtonReleased(BUTTON_Y)) {
pickup_O_direction = pickup_O_direction_prev;
}
if (Joystick_ButtonReleased(BUTTON_X)) {
pickup_O_direction = pickup_O_direction_prev;
}
if (Joystick_ButtonPressed(BUTTON_RB)) {
switch (pickup_O_direction) {
case DIRECTION_NONE :
case DIRECTION_OUT :
pickup_O_direction = DIRECTION_IN;
pickup_I_direction = DIRECTION_IN;
break;
case DIRECTION_IN :
pickup_O_direction = DIRECTION_NONE;
pickup_I_direction = DIRECTION_NONE;
break;
}
}
if (Joystick_ButtonPressed(BUTTON_RT)) {
pickup_O_direction_prev = pickup_O_direction;
pickup_I_direction_prev = pickup_I_direction;
pickup_O_direction = DIRECTION_OUT;
pickup_I_direction = DIRECTION_OUT;
}
if (Joystick_ButtonReleased(BUTTON_RT)) {
pickup_O_direction = pickup_O_direction_prev;
pickup_I_direction = pickup_I_direction_prev;
}
if (Joystick_DirectionPressed(DIRECTION_R)) {
pickup_pos = PICKUP_RETRACT;
} else if (Joystick_DirectionPressed(DIRECTION_F)) {
pickup_pos = PICKUP_KICK;
} else if (Joystick_DirectionPressed(DIRECTION_L)) {
pickup_pos = PICKUP_LARGE;
} else if (Joystick_DirectionPressed(DIRECTION_B)) {
pickup_pos = PICKUP_SMALL;
}
if (Joystick_Button(BUTTON_LB)) {
clamp_direction = DIRECTION_OUT;
} else if (Joystick_Button(BUTTON_LT)) {
clamp_direction = DIRECTION_IN;
} else {
clamp_direction = DIRECTION_NONE;
}
if (Joystick_Button(BUTTON_RB, CONTROLLER_2)) {
servo_dump_pos = pos_servo_dump_open_small;
} else if (Joystick_Button(BUTTON_RT, CONTROLLER_2)) {
servo_dump_pos = pos_servo_dump_open_large;
} else {
servo_dump_pos = pos_servo_dump_closed;
}
if (pickup_I_direction==DIRECTION_IN && lift_pos<pos_dump_safety) {
servo_dump_pos = pos_servo_dump_open_feed;
}
if (Joystick_Button(BUTTON_LB, CONTROLLER_2)) {
clamp_direction = DIRECTION_IN;
}
if (Joystick_Button(BUTTON_LT, CONTROLLER_2)) {
clamp_direction = DIRECTION_OUT;
}
if (Joystick_ButtonPressed(BUTTON_X, CONTROLLER_2)) {
lift_target = pos_lift_bottom;
is_lift_manual = false;
hopper_target = pos_servo_hopper_down;
servo_turntable_pos = pos_servo_turntable_F;
isLiftFrozen = true;
servo_dump_pos = pos_servo_dump_closed;
} else if (Joystick_ButtonPressed(BUTTON_A, CONTROLLER_2)) {
lift_target = pos_lift_low;
is_lift_manual = false;
hopper_target = pos_servo_hopper_goal;
servo_turntable_pos = pos_servo_turntable_F;
isHopperFrozen = true;
servo_dump_pos = pos_servo_dump_closed;
} else if (Joystick_ButtonPressed(BUTTON_B, CONTROLLER_2)) {
lift_target = pos_lift_medium;
is_lift_manual = false;
hopper_target = pos_servo_hopper_goal;
servo_turntable_pos = pos_servo_turntable_F;
isHopperFrozen = true;
servo_dump_pos = pos_servo_dump_closed;
} else if (Joystick_ButtonPressed(BUTTON_Y, CONTROLLER_2)) {
lift_target = pos_lift_high;
is_lift_manual = false;
hopper_target = pos_servo_hopper_goal;
servo_turntable_pos = pos_servo_turntable_F;
isHopperFrozen = true;
servo_dump_pos = pos_servo_dump_closed;
}
if (Joystick_ButtonPressed(BUTTON_START)) {
Servo_SetPosition(servo_hopper_A, pos_servo_hopper_goal);
Servo_SetPosition(servo_hopper_B, pos_servo_hopper_goal);
if (abs(hopper_target-pos_servo_hopper_center)<3) {
hopper_target = pos_servo_hopper_down;
} else {
hopper_target = pos_servo_hopper_center;
}
}
if (Joystick_ButtonPressed(BUTTON_BACK)) {
Servo_SetPosition(servo_hopper_A, pos_servo_hopper_center);
Servo_SetPosition(servo_hopper_B, pos_servo_hopper_center);
if (abs(hopper_target-pos_servo_hopper_goal)<3) {
hopper_target = pos_servo_hopper_down;
} else {
hopper_target = pos_servo_hopper_goal;
}
}
if (isLiftFrozen && hopper_pos < pos_servo_hopper_up) {
isLiftFrozen = false;
}
if (isHopperFrozen) {
hopper_target = pos_servo_hopper_up;
if (abs(lift_pos-lift_target)<300) {
hopper_target = pos_servo_hopper_goal;
isHopperFrozen = false;
}
}
switch (pickup_I_direction) {
case DIRECTION_NONE :
power_pickup_I = 0;
break;
case DIRECTION_IN :
power_pickup_I = 100;
break;
case DIRECTION_OUT :
power_pickup_I = -100;
break;
}
switch (pickup_O_direction) {
case DIRECTION_NONE :
power_pickup_O = 0;
break;
case DIRECTION_IN :
power_pickup_O = 100;
break;
case DIRECTION_OUT :
power_pickup_O = -100;
break;
}
switch (clamp_direction) {
case DIRECTION_NONE :
power_clamp = 0;
break;
case DIRECTION_IN :
power_clamp = 100;
break;
case DIRECTION_OUT :
power_clamp = -100;
break;
}
Motor_SetPower(power_L, motor_LT);
Motor_SetPower(power_L, motor_LB);
Motor_SetPower(power_R, motor_RT);
Motor_SetPower(power_R, motor_RB);
Motor_SetPower(power_pickup_I, motor_pickup_I);
Motor_SetPower(power_pickup_O, motor_pickup_O);
Motor_SetPower(power_clamp, motor_clamp_L);
Motor_SetPower(power_clamp, motor_clamp_R);
Servo_SetPosition(servo_dump, servo_dump_pos);
// NOTE: Hopper and turntable servos should be set in the "Hopper" task.
switch (pickup_pos) {
case PICKUP_RETRACT :
Servo_SetPosition(servo_pickup_L, 129 + pos_servo_pickup_retract);
Servo_SetPosition(servo_pickup_R, 120 - pos_servo_pickup_retract);
break;
case PICKUP_LARGE :
Servo_SetPosition(servo_pickup_L, 129 + pos_servo_pickup_large);
Servo_SetPosition(servo_pickup_R, 120 - pos_servo_pickup_large);
break;
case PICKUP_SMALL :
Servo_SetPosition(servo_pickup_L, 129 + pos_servo_pickup_small);
Servo_SetPosition(servo_pickup_R, 120 - pos_servo_pickup_small);
break;
case PICKUP_KICK :
Servo_SetPosition(servo_pickup_L, 129 + pos_servo_pickup_kick);
Servo_SetPosition(servo_pickup_R, 120 - pos_servo_pickup_kick);
break;
}
Time_Wait(5);
}
}
/******************************************************************************
*
* Test program
*
*****************************************************************************/
int
main(void)
{
int ret;
int servo; // lame TI compiler cant handle loop var declaration
FPUStackingDisable();
/* Initialize the clock to run at 40 MHz
*/
SysCtlClockSet(SYSCTL_SYSDIV_5 | SYSCTL_USE_PLL | SYSCTL_XTAL_16MHZ | SYSCTL_OSC_MAIN);
gSysClock = SysCtlClockGet();
/* Initialize the UART.
*/
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOA);
GPIOPinConfigure(GPIO_PA0_U0RX);
GPIOPinConfigure(GPIO_PA1_U0TX);
GPIOPinTypeUART(GPIO_PORTA_BASE, GPIO_PIN_0 | GPIO_PIN_1);
#ifdef STELLARISWARE
UARTStdioInit(0);
#else
UARTStdioConfig(0, 115200, gSysClock);
#endif
UARTprintf("\n\nServoBoard-Test\n---------------\n");
/* Initialize the GPIO port for the RGB LED
*/
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOF);
GPIOPinTypeGPIOOutput(GPIO_PORTF_BASE, RGB_PINS);
GPIOPinWrite(GPIO_PORTF_BASE, RGB_PINS, 0);
/* Initialize the battery monitor
* Use zeroes for parameter so default calibration will be used
*/
Servo_BatteryInit(0, 0);
/* Initialize servos for 20 msec
*/
ret = Servo_Init(gSysClock, 20000);
if(ret)
{
UARTprintf("error calling ServoInit\n");
return 0;
}
/* Enter loop to initialize all the servos in the system
*/
for(servo = 0; servo < NUM_SERVOS; servo++)
{
/* Associate each servo ID with a hardware timer (and A or B half)
*/
hServo[servo] = Servo_Config(servoInfo[servo].timer, servoInfo[servo].half);
if(hServo[servo] == 0)
{
UARTprintf("error config servo %d\n", servo);
return 0;
}
/* Delay a bit before initting the next servo. This is done to
* spread out the servo pulses so they do not all happen at the
* same time and load down the power supply.
* The delay value was determined experimentally. If the
* system clock frequency is changed then the delay value needs to
* be changed
*/
SysCtlDelay(22000);
}
/* Set each servo position to 0 to start, with 100 ms delay
*/
for(servo = 0; servo < NUM_SERVOS; servo++)
{
/* Set the servo motion rate */
Servo_SetMotionParameters(hServo[servo], 200);
Servo_SetPosition(hServo[servo], 0);
SysCtlDelay((gSysClock / 10) / 3);
}
// MoveAll(0xFFF, 0);
/* In this loop we just move all the servos between +45 and
* -45 deg (uncalibrated). There is a 100 ms delay between each
* servo, so that if observed with a scope each servo does not have
* the exact same timing.
*/
while(1)
{
/* Move all servos to -45 deg, with 100 ms between each servo
*/
for(servo = 0; servo < NUM_SERVOS; servo++)
{
UpdateRGB();
MoveOne(servo, -450);
DelayMs(100);
}
/* Now move all servos to +45 deg, with 100 ms delay
*/
for(servo = 0; servo < NUM_SERVOS; servo++)
{
UpdateRGB();
MoveOne(servo, 450);
DelayMs(100);
}
/* Read the battery voltage and print to the terminal
*/
uint32_t bat = Servo_ReadBatteryMv();
UARTprintf("%u.%02u V\n", bat / 1000, (bat % 1000) / 10);
}
#ifndef ccs // prevent warning from TI ccs compiler
return(0);
#endif
}
