int main(int argc,char **argv) {
int i; //iterator
pid *x,*y,*z; //structs representing 3 pid controllers
imu_t *imu;
fd_set net_set;
double oldmag,magdiff;
struct timeval timeout;
int axes[4]={0,0,0,0};
int x_adjust,y_adjust,z_adjust;
int sock = init_net();
register_int();
x=init_pid(3500,000,000);
y=init_pid(3400,000,1000);
z=init_pid(000,000,000);
timeout.tv_sec=0;
timeout.tv_usec=REFRESH_TIME;
for(i=0;i<4;i++) { //initialize PWM modules
motors[i]=init_pwm(i);
}
imu = init_imu();
update_imu(imu);
oldmag=imu->mpu.fusedEuler[VEC3_Z] * RAD_TO_DEGREE;
while(1) {
////printf("loop\n");
FD_ZERO(&net_set);
FD_SET(sock,&net_set);
if(select(sock+1,&net_set,(fd_set *) 0,(fd_set *) 0, &timeout)==1) {
////printf("reading from the socket\n");
//read from sock
if(!update_axis(sock,axes)) {
break;
}
} else {
////printf("reading from the imu\n");
//update imu
update_imu(imu);
timeout.tv_sec=0;
timeout.tv_usec=REFRESH_TIME;
magdiff=imu->mpu.fusedEuler[VEC3_Z] * RAD_TO_DEGREE-oldmag;
if(magdiff>300) {
magdiff-=360;
}
if(magdiff<-300) {
magdiff+=360;
}
x_adjust = update_pid(x,0/*axes[1]*.00061*/,imu->mpu.fusedEuler[VEC3_X] * RAD_TO_DEGREE);
y_adjust = update_pid(y,0/*axes[2]*.00061*/,imu->mpu.fusedEuler[VEC3_Y] * RAD_TO_DEGREE);
//printf("X axis value: %lf\n",imu->mpu.fusedEuler[VEC3_X]*RAD_TO_DEGREE);
//printf("Y axis value: %lf\n",imu->mpu.fusedEuler[VEC3_Y]*RAD_TO_DEGREE);
z_adjust = update_pid(z,axes[3]*.00061,magdiff);
oldmag=imu->mpu.fusedEuler[VEC3_Z] * RAD_TO_DEGREE;
set_duty(motors[0],900000+axes[0]*16-z_adjust-y_adjust+x_adjust);
set_duty(motors[1],900000+axes[0]*16+z_adjust-y_adjust-x_adjust);
set_duty(motors[2],900000+axes[0]*16-z_adjust+y_adjust-x_adjust);
set_duty(motors[3],900000+axes[0]*16+z_adjust+y_adjust+x_adjust);
}
}
exit_fxn(0);
}
int umain(){
robot_id = 1;
copy_objects();
copy_objects();
field_state.curr_loc.x = (int)(game.coords[0].x * VPS_RATIO);
field_state.curr_loc.y = (int)(game.coords[0].y * VPS_RATIO);
field_state.curr_angle = (((float)game.coords[0].theta) * 180.0) / 2048;
gyro_set_degrees(field_state.curr_angle);
initialize();
/*for(int sextant = 0; sextant < 6; sextant++){
printf("TPX: %d, TPY: %d, LPX: %d, LPY: %d\n", get_territory_pivot_point_loc(sextant).x, get_territory_pivot_point_loc(sextant).y, lever_pivot_points[2*sextant], lever_pivot_points[2*sextant + 1]);
}*/
init_pid(&field_state.circle_PID, KP_CIRCLE, KI_CIRCLE, KD_CIRCLE, &get_angle_error_circle, &update_circle_velocities);
field_state.circle_PID.enabled = true;
field_state.circle_PID.goal = 0;
//printf("OX: %d, OY: %d, TX: %d, TY: %d, TWX: %d, TWY: %d, CA: %.2f, G: %.3f, LE: %d, RE: %d\n", field_state.curr_loc.x, field_state.curr_loc.y, field_state.target_loc.x, field_state.target_loc.y, field_state.target_loc_waypoint.x, field_state.target_loc_waypoint.y, field_state.curr_angle, gyro_get_degrees(), encoder_read(LEFT_ENCODER_PORT), encoder_read(RIGHT_ENCODER_PORT));
while(1){
update_field();
accelerate();
decelerate();
if(field_state.pid_enabled)
update_pid(&field_state.circle_PID);
if(get_time() > 119000){
}
//printf("Sextant: %d, OX: %d, OY: %d, TX: %d, TY: %d, TWX: %d, TWY: %d, CA: %.2f, G: %.3f, LE: %d, RE: %d\n", get_current_sextant(field_state.curr_loc), field_state.curr_loc.x, field_state.curr_loc.y, field_state.target_loc.x, field_state.target_loc.y, field_state.target_loc_waypoint.x, field_state.target_loc_waypoint.y, field_state.curr_angle, gyro_get_degrees(), encoder_read(LEFT_ENCODER_PORT), encoder_read(RIGHT_ENCODER_PORT));
}
return 0;
}
bool navigateToTarget() {
//return true if need to be called next frame
if (get_time() - state_time > 2800) {
determineTargetPosition();
updateSelfPosition(true);
updateAngleToTarget();
state_time = get_time();
}
else if (get_time() - state_time > 2500) {
motor_set_vel(RIGHT_MOTOR,0);
motor_set_vel(LEFT_MOTOR,0);
return true;
}
if (get_time() - last_update_time > 100) {
updateSelfPosition(false);
determineTargetPosition();
updateAngleToTarget();
}
if (getDistanceToTarget(target_x,target_y) < 4.0) {
motor_set_vel(RIGHT_MOTOR,0);
motor_set_vel(LEFT_MOTOR,0);
resetPID(0);
return false;
}
update_pid(&driver);
return true;
}
void main(void)
{
init();
PSC_Init(0x00, MAX_PWM);
uart_init(UART_BAUD_SELECT_DOUBLE_SPEED(UART_BAUDRATE, F_CPU));
while(1)
{
/* Communicate with Big Brother :-) */
communication();
/* Sampling */
if (Flag_IT_timer0)
{
if ( !(timer % 100) )
{
/* This led shows that program is running */
GPIO_TOGGLE(WD_LED);
}
if ( !(timer % 10) )
{
GPIO_TOGGLE(LED);
/* Read meashured speed from Timer2 */
Omega_meas = TCNT1 * 10;
TCNT1 = 0;
/* PID controller */
pid_output = update_pid(reference_val.omega - Omega_meas, Omega_meas);
Command = ((int32_t)pid_output * 131) >> 10;
/* Update immediate vals */
instant_val.time = timer;
instant_val.omega = Omega_meas;
instant_val.error = reference_val.omega - Omega_meas;
instant_val.pid_output = pid_output;
instant_val.command = Command;
}
/* direction management : extract sign and absolute value */
if (Command > (int16_t)(0))
{
direction = 0;
OmegaTe = Command;
} else {
direction = 1;
OmegaTe = (~Command) + 1;
}
if (OmegaTe > K_scal)
{
// ------------------------ V/f law --------------------------
amplitude = controlVF(OmegaTe);
// ------------------ natural PWN algorithm ------------------
PWM0 = duty_cycle(theta1,amplitude);
PWM1 = duty_cycle(theta2,amplitude);
PWM2 = duty_cycle(theta3,amplitude);
} else {
PWM0 = 0;
PWM1 = 0;
PWM2 = 0;
}
// -------- load the PSCs with the new duty cycles -----------
PSC0_Load(PWM0, PWM0 + DeadTime);
if (!direction)
{
PSC1_Load(PWM1, PWM1 + DeadTime);
PSC2_Load(PWM2, PWM2 + DeadTime);
} else {
PSC2_Load(PWM1, PWM1 + DeadTime);
PSC1_Load(PWM2, PWM2 + DeadTime);
}
// 3 integrators for the evolution of the angles
theta1 = (K_scal * theta1 + OmegaTe) / K_scal;
theta2 = theta1 + 160;
theta3 = theta1 + 320;
if (theta1>=MAX_THETAx4) theta1 -= MAX_THETAx4;
if (theta2>=MAX_THETAx4) theta2 -= MAX_THETAx4;
if (theta3>=MAX_THETAx4) theta3 -= MAX_THETAx4;
Flag_IT_timer0 = 0;
}
}
void mainLoop()
{
while(1) {
//pid processing and output debug msg
if (get_flag(FLAG_END_ADC_CONV)) {
temp_error = temp_adc[temp_lvl_real] - conv_result;
if ( get_flag(FLAG_PREHEAT) && (temp_error<5) ) { //check if preheat phase completed
temp_lvl_real = temp_lvl;
reset_flag(FLAG_PREHEAT);
}
if ( get_flag(FLAG_BLINK_ON) && ((get_flag(FLAG_PREHEAT))==0) ) { //BLINK_OFF if temp stabilized
if ( (temp_error>-5) && (temp_error<5) ) {
LEDS_SET(temp_lvl);
reset_flag(FLAG_BLINK_ON);
}
}
//calculate error for pid
temp_error = temp_adc[temp_lvl_real] - conv_result;
if (temp_error > 127)
temp_error=127; // for int8_t in pid
else if (temp_error<-127)
temp_error=-127;
heat_cycles = update_pid(&pid, (int8_t)(temp_error), conv_result);
reset_flag(FLAG_END_ADC_CONV);
//DBG
// sprintf(txbuf,"%d,%d %d %d %x\r\n",conv_result,(uint16_t)(temp_lvl), (int16_t)heat_cycles, (int16_t)((int8_t)(temp_error)), (uint16_t)(flags));
// UART_SendStr(txbuf); //send result to uart
}
//zero-detection time-non-critical events processiong
if (get_flag(FLAG_ZERO_REACHED)) {
reset_flag(FLAG_ZERO_REACHED);
phase_ticks++;
//leds blink
if (get_flag(FLAG_BLINK_ON)) {
if ( (phase_ticks & 0b011111) == 0 ) {
LEDS_SET(temp_lvl);
} else if ( (phase_ticks & 0b100000) == 0 ) {
LEDS_SET(0);
}
}
//long switch-pressed: pwr-off
if (get_flag(FLAG_SWITCH_PRESSED)) {
switch_pressed_timer++;
// if ( switch_pressed_timer == 0 ) {
if ( switch_pressed_timer == 0x90 ) {
switch_pressed_timer=0;
temp_lvl=0;
temp_lvl_real=0;
LEDS_SET(0);
}
}
//if key is pressed
if ( ((SWITCH_PORT->IDR & SWITCH_PIN)==0) && ((get_flag(FLAG_SWITCH_PRESSED))==0) ) {
set_flag(FLAG_SWITCH_PRESSED);
switch_pressed_timer=0;
temp_lvl = (temp_lvl+1) % 8;
LEDS_SET(temp_lvl);
set_flag(FLAG_BLINK_ON);
set_flag(FLAG_PREHEAT);
pid.pgain=pgains[temp_lvl];
pid.igain=igains[temp_lvl];
if ((temp_lvl<7) && (temp_lvl>0)) {
temp_lvl_real = temp_lvl+1;
} else {
reset_flag(FLAG_PREHEAT);
reset_flag(FLAG_BLINK_ON);
temp_lvl_real = temp_lvl;
}
//if key is released (pull-up)
} else if ( (SWITCH_PORT->IDR & SWITCH_PIN) && get_flag(FLAG_SWITCH_PRESSED) ) {
reset_flag(FLAG_SWITCH_PRESSED);
}
}
} // end while(1)
}
void PIDUpdate() {
update_pid(&ForwardPID);
update_pid(&RingPID);
//ForwardPID.goal = target;
//pause(PID_DELAY);
}