/*****************************************
* Main function - it is not necessary to
* modify this
*****************************************/
task main()
{
/* Reset encoders and turn on PID control */
setup();
nMotorEncoder[motorB] = 0;
nMotorEncoder[motorC] = 0;
nMotorPIDSpeedCtrl[motorB] = mtrSpeedReg;
nMotorPIDSpeedCtrl[motorC] = mtrSpeedReg;
nPidUpdateInterval = PIDUpdateInterval;
float tempx = pen_X;// - cos(robot_TH)*LX;
float tempy = pen_Y;// - sin(robot_TH)*LX;
float tempTH = robot_TH;
float theta;
float i = 0.0;
while (1) {
/*theta = -1.0*PI/60.0*i +tempTH;
target_x = cos(theta)*LX - sin(theta)*LY + tempx;
target_y = sin(theta)*LX + cos(theta)*LY + tempy;*/
target_x = tempx + i;
target_y = tempy + i;
writeDebugStreamLine("%f %f %f %f\n", target_x, target_y, pen_X, pen_Y);
dead_reckoning();
pid_loop();
wait1Msec(200);
i = i+0.01;
}
}
/*********************************************************************
Main Program Loop
**********************************************************************/
int main()
{
/* Initializations */
debug_init(); /* This should be first. */
timer_init();
/* This should be before any GPIO activities. */
uint32 ret_val = bcm2835_init();
if ( ret_val == 0 )
{
DEBUG_MSG_ERROR("bcm2835_init() failed.");
}
pwm_init();
pump_init();
therm_init();
pid_init();
pump_start();
/* Take temperature as input from console. */
float setpoint;
printf("Set your desired temperature: ");
scanf("%f", &setpoint);
pid_update_temp_setpoint(setpoint);
pid_gain_params pid_gain;
pid_gain.k_p = 1;
pid_gain.k_d = 1;
pid_gain.k_i = 1;
pid_gain.k_windup = 1;
pid_set_gain(&pid_gain);
/* Main Program Loop */
while (1)
{
pwm_run();
therm_capture();
pid_loop();
}
pump_stop();
/* De-initializations */
pump_deinit();
pid_deinit();
pwm_deinit();
/* This should be after all GPIO activities. */
ret_val = bcm2835_close();
if ( ret_val == 0 )
{
DEBUG_MSG_ERROR("bcm2835_close() failed.");
}
timer_deinit();
debug_deinit(); /* This should be last. */
return 0;
}
