void update_circle_velocities(float error){
int pwr_change = format_velocity(fabs(error));
int dir = field_state.drive_direction;
if(field_state.substage == DRIVE_SUBSTAGE){
/*if(fabs(mod_f(field_state.target_angle - mod_f(gyro_get_degrees(), 180), 180)) < ROTATION_THRESHOLD){
if(current_vel == 0)
current_vel = target_vel;
}*/
if(error*dir > 0){
set_motors(dir*current_vel, dir*current_vel - dir*pwr_change);
//printf("Left Vel: %d, Right Vel: %d\n", current_vel, current_vel - pwr_change);
}
else{
set_motors(dir*current_vel - dir*pwr_change, dir*current_vel);
//printf("Left Vel: %d, Right Vel: %d\n", current_vel - pwr_change, current_vel);
}
}
else if(field_state.substage == TERRITORY_APPROACH_SUBSTAGE || field_state.substage == LEVER_APPROACH_SUBSTAGE || field_state.substage == DUMPING_SUBSTAGE || field_state.substage == PIVOT_SUBSTAGE || field_state.substage == LEVER_RETREAT_SUBSTAGE || field_state.substage == TERRITORY_RETREAT_SUBSTAGE){
if(fabs(mod_f(field_state.target_angle - mod_f(gyro_get_degrees(), 180), 180)) < ROTATION_THRESHOLD){
if(current_vel == 0){
//used to know when we started moving toward the lever/territory
field_state.stored_time = field_state.curr_time;
current_vel = target_vel;
}
}
if(error*dir > 0){
set_motors(dir*current_vel + dir*pwr_change, dir*current_vel - dir*pwr_change);
//printf("Left Vel: %d, Right Vel: %d\n", current_vel, current_vel - pwr_change);
}
else{
set_motors(dir*current_vel - dir*pwr_change, dir*current_vel + dir*pwr_change);
//printf("Left Vel: %d, Right Vel: %d\n", current_vel - pwr_change, current_vel);
}
}
}
void accelerate(){
if(is_decelerating())
return;
else if(current_vel < target_vel && (field_state.substage == DRIVE_SUBSTAGE || field_state.substage == DUMPING_SUBSTAGE)){
if(field_state.curr_time < field_state.start_drive_time + accelerate_time){
current_vel = format_velocity(scale_values(field_state.curr_time - field_state.start_drive_time, accelerate_time, target_vel));
}
else{
current_vel = target_vel;
}
}
}
void format_string_code(unsigned int format_code, char **dest, char **args)
{
int value;
switch (format_code) {
case FORMAT_COMMA32:
// Pop argument
value = *((sint32*)*args);
*args += 4;
format_comma_separated_integer(dest, value);
break;
case FORMAT_INT32:
// Pop argument
value = *((sint32*)*args);
*args += 4;
format_integer(dest, value);
break;
case FORMAT_COMMA2DP32:
// Pop argument
value = *((sint32*)*args);
*args += 4;
format_comma_separated_fixed_2dp(dest, value);
break;
case FORMAT_COMMA1DP16:
// Pop argument
value = *((sint16*)*args);
*args += 2;
format_comma_separated_fixed_1dp(dest, value);
break;
case FORMAT_COMMA16:
// Pop argument
value = *((sint16*)*args);
*args += 2;
format_comma_separated_integer(dest, value);
break;
case FORMAT_UINT16:
// Pop argument
value = *((uint16*)*args);
*args += 2;
format_integer(dest, value);
break;
case FORMAT_CURRENCY2DP:
// Pop argument
value = *((sint32*)*args);
*args += 4;
format_currency_2dp(dest, value);
break;
case FORMAT_CURRENCY:
// Pop argument
value = *((sint32*)*args);
*args += 4;
format_currency(dest, value);
break;
case FORMAT_STRINGID:
case FORMAT_STRINGID2:
// Pop argument
value = *((uint16*)*args);
*args += 2;
format_string_part(dest, value, args);
(*dest)--;
break;
case FORMAT_STRING:
// Pop argument
value = *((uint32*)*args);
*args += 4;
if (value != 0) {
strcpy(*dest, (char*)value);
*dest += strlen(*dest);
}
break;
case FORMAT_MONTHYEAR:
// Pop argument
value = *((uint16*)*args);
*args += 2;
format_date(dest, value);
break;
case FORMAT_MONTH:
// Pop argument
value = *((uint16*)*args);
*args += 2;
strcpy(*dest, language_get_string(STR_MONTH_MARCH + date_get_month(value)));
*dest += strlen(*dest);
break;
case FORMAT_VELOCITY:
// Pop argument
value = *((sint16*)*args);
*args += 2;
format_velocity(dest, value);
break;
case FORMAT_POP16:
*args += 2;
break;
case FORMAT_PUSH16:
*args -= 2;
break;
case FORMAT_DURATION:
// Pop argument
value = *((uint16*)*args);
*args += 2;
format_duration(dest, value);
break;
case FORMAT_REALTIME:
// Pop argument
value = *((uint16*)*args);
*args += 2;
format_realtime(dest, value);
break;
case FORMAT_LENGTH:
// Pop argument
value = *((sint16*)*args);
*args += 2;
format_length(dest, value);
break;
case FORMAT_SPRITE:
// Pop argument
value = *((uint32*)*args);
*args += 4;
*(*dest)++ = 23;
*((uint32*)(*dest)) = value;
*dest += 4;
break;
}
}
