/* Start user code for adding. Do not edit comment generated here */
void packet_process(void)
{
int pwm; /* duty cycle for duty assignment mode */
int mode; /* assigned mode from master packet */
/* if ignore bit set, do nothing */
if (masterPacket[0] & 2)
return;
mode = masterPacket[0] & 12;
/* servo off mode */
if (mode == 0)
{
set_motor_output(0);
TRG = 0;
count = 0;
}
/* duty assignment mode */
if (mode == 4)
{
pwm = (double)((int8_t)masterPacket[3]) / 100 * 3100;
set_motor_output(pwm);
}
/* position control mode */
if (mode == 8)
{
pid_control();
}
/* else reservation mode, don't need to do anything */
/* if this is the time we are processing this packet, and if required send reply */
if (firstCall == 1 && masterPacket[1] & 1 == 1)
{
firstCall = 0;
bufferOut[0] = masterPacket[0];
bufferOut[2] = count >> 8;
bufferOut[3] = count;
bufferOut[4] = crc((((long)bufferIn[0]) << 24) +
(((long)bufferIn[1]) << 16) +
(((long)bufferIn[2]) << 8) +
((long) bufferIn[3]));*/
P1.5 = 1;
R_UART2_Send(bufferOut , 5);
/* when send finishes, it will toggle DE for receive */
}
/************************************************************
motor_timeslice() Call this every 10ms
Read Position Also update Speed & Acceleration
update duty (if pid)
compare to limit
compare to destination
send postion report
turn off if shutdown
*************************************************************/
void motor_timeslice_10ms()
{
check_limit_disable(0);
check_limit_disable(1);
check_limit_disable(2);
check_limit_disable(3);
check_limit_disable(4);
if (motors_stopped==FALSE)
for (int i=0; i<NUM_MOTORS; i++)
{
if (mot_states[i].PID_control)
pid_control(i);
}
//ramp_duty();
}
float yaw_ctrl_step(float *err_out, float yaw, float _speed, float dt)
{
float err;
float yaw_ctrl;
if (tsint_get(&manual))
{
yaw_ctrl = 0.0f;
err = 0.0; /* we control nothing, so the error is always 0 */
}
else
{
err = circle_err(yaw, tsfloat_get(&pos));
float speed_setpoint = -speed_func(err);
float speed_err = speed_setpoint - _speed;
yaw_ctrl = pid_control(&controller, speed_err, dt);
}
*err_out = err;
return yaw_ctrl;
}
static void wrapper_pid(void * pidData) {
uint8_t pid_out = pid_control(set_point, (pidData_t*) pidData); //setpoint 100Hz
pwm_setDutyCycle(pid_out);
fprintf(uartout, "real dutycycle: %d\n", pid_out);
}
void init() {
/* Load persistent settings, or set the default values. */
if(persistent_load() == -1) {
wanted_temperature = TEMP_DEF;
wanted_humidity = HUM_DEF;
}
i2c_init();
httpd_init();
logs_init();
pid_init();
struct sigaction sa = (struct sigaction) {
.sa_handler = &reload,
.sa_flags = 0,
.sa_restorer = NULL
};
sigemptyset(&(sa.sa_mask));
sigaddset(&(sa.sa_mask), SIGUSR1);
sigaddset(&(sa.sa_mask), SIGINT);
sigaddset(&(sa.sa_mask), SIGTERM);
if(sigaction(SIGUSR1, &sa, NULL) < 0) {
fprintf(stderr, "Couldn't install signal handler.\n");
exit(1);
}
sa.sa_handler = &end;
if(sigaction(SIGTERM, &sa, NULL) < 0 || sigaction(SIGINT, &sa, NULL) < 0) {
fprintf(stderr, "Couldn't install signal handler.\n");
exit(1);
}
}
void reload(int signal) {
httpd_reload();
}
int main(int argc, char **argv) {
float data[2];
init();
struct timespec ts;
clock_gettime(CLOCK_MONOTONIC, &ts);
for(;;) {
if(!read_data(data)) {
continue;
}
log_values(data[1], data[0]);
pid_control(data[1], data[0]);
stats();
if(logstdout(data) == EOF) {
break;
}
delay_ns(&ts, PERIOD_S, 0);
}
end();
}
FOR_EACH(i, controllers)
{
err->ve[i] = sym_limit(setp->ve[i], deg2rad(tsfloat_get(&angles_max)))
+ deg2rad(tsfloat_get(&biases[i])) - pos->ve[i];
ctrl->ve[i] = pid_control(&controllers[i], err->ve[i], speed->ve[i], dt);
}
float u_ctrl_step(float *err, const float setpoint, const float pos, const float speed, const float dt)
{
*err = setpoint - pos;
return pid_control(&ctrl, *err, speed, dt);
}
/*******************************************************************************
* FUNCTION NAME: Default_Routine
* PURPOSE: Performs the default mappings of inputs to outputs for the
* Robot Controller.
* CALLED FROM: this file, Process_Data_From_Master_uP routine
* ARGUMENTS: none
* RETURNS: void
*******************************************************************************/
void Default_Routine(void)
{
static int temp_angle = 0;
//%d = decimal
//%i = integer
//%li = long integer
encoder_1_count = (int)Get_Encoder_1_Count();
encoder_2_count = (int)Get_Encoder_2_Count();
pan_gyro_angle = Get_Gyro_Angle();
//debug
printf("ARM: %i | WRIST: %i | cam tilt %i | cam_pan : %i | M: %li %i\r\n", encoder_1_count, encoder_2_count, PAN_SERVO, TILT_SERVO, Get_Gyro_Angle(), Get_ADC_Result(2));
//printf("%i %i %i %i", auto_switch_1, auto_switch_2, auto_switch_3, auto_switch_4)
//printf("\r\nauto_switch_1: %i | auto_switch_2: %i | auto_switch_3: %i | auto_switch_4: %i", auto_switch_1, auto_switch_2, auto_switch_3, auto_switch_4);
//DRIVETRAIN CONTROL (arcade drive)
if (!p4_sw_aux2 && !p4_sw_top) {
drive_R1 = drive_R2 = Limit_Mix(2000 + (p3_x) + p3_y - 127);
drive_L1 = drive_L2 = flip_axis(Limit_Mix(2000 + (p3_x) - p3_y + 127), 127);
tar_prev = 0;
desired_robot_angle = 0;
}else if (p4_sw_top) {
if (tar_prev == 0) {
tar_prev = 1;
desired_robot_angle = pan_gyro_angle;
//printf("\nEntered drive mode\n");
}
temp_angle = pid_control(&gyro_c, pan_gyro_angle - desired_robot_angle);
drive_R1 = drive_R2 = Limit_Mix(2000 + (temp_angle) + p3_y - 127);
drive_L1 = drive_L2 = flip_axis(Limit_Mix(2000 + (temp_angle) - p3_y + 127), 127);
}else if (p4_sw_aux2) {
drive_R1 = drive_R2 = Limit_Mix(2000 + (p3_x) + flip_axis(p3_y, 127) - 127);
drive_L1 = drive_L2 = flip_axis(Limit_Mix(2000 + (p3_x) - flip_axis(p3_y, 127) + 127), 127);
}
/*else{
drive_R1 = drive_L1 = pid_control(&Mr_Roboto, pan_gyro_angle - desired_robot_angle);
}*/
//printf("gy: %i | %i | %i\r\n", drive_R1, drive_L1, tar_prev);
//if (tar_prev && !p4_sw_aux2) {
// tar_prev = 0;
//}
//printf("pid, dones (arm, wrist, dist, angle) (%i)", score_pos);
////**V*V*V*V*V*V*V*V*V*V*V*V*V*V*V*V*V*V*V*V*V*V*V*V*V*V
//SET THE ARM POS
if (p2_sw_aux2 == 1) { //UPPER
set_arm_pos(ARM_TOP, WRIST_TOP);
score_pos = score_top;
}else if(p2_sw_aux1 == 1) { //MIDDLE
set_arm_pos(ARM_MID, WRIST_MID);
score_pos = score_mid;
}else if(p2_sw_top == 1) { //LOWER
set_arm_pos(ARM_LOW, WRIST_LOW);
score_pos = score_low;
}else if (p2_sw_trig == 1) { //HOME
set_arm_pos(ARM_HOME, WRIST_HOME);
}
//Zach's Absolute Arm Positions
//set_arm_pos(p3_y, WRIST_HOME);
//where_i_want_to_be -= (((int)p1_y - 127)/30); //PRECISION ARM
//desired_wrist_pos += (((int)p2_y - 127)/20); //PRECISION WRIST
if (where_i_want_to_be > ARM_MAX) { //ARM LIMIT
where_i_want_to_be = ARM_MAX;
}else if (where_i_want_to_be < ARM_MIN) {
where_i_want_to_be = ARM_MIN;
}
//END SET ARM POS
////**^*^*^*^*^*^*^*^*^*^^*^*^*^*^*^*^*^*^*^*^*^*^*^*^*^*^*^*^*^*
if (p1_sw_aux1) { //Set pickup position
zach_var = encoder_2_count;
}
if (p1_sw_trig > 0) { //SAFETY TRIGGER
arm_l_motor = arm_r_motor = p1_y;
wrist_motor = flip_axis(p2_y, 127);
where_i_want_to_be = encoder_1_count;
desired_wrist_pos = encoder_2_count;
}else if (able_to_correct) { //CONSTANT CONTROL
arm_l_motor = arm_r_motor = pid_control(&arm, where_i_want_to_be - encoder_1_count);
wrist_motor = pid_control(&wrist, desired_wrist_pos - encoder_2_count);
}else if (p1_sw_top == 1) {
arm_l_motor = arm_r_motor = p1_y;
if (p4_sw_trig) {
desired_wrist_pos = (3 * encoder_1_count)/4 + 276 - ((int)p2_y - 127); //insert formula here.
}else{
desired_wrist_pos = zach_var; //ZACH_AND_ELLEN_RULE //245
}
wrist_motor = pid_control(&wrist, desired_wrist_pos - encoder_2_count);
}else{
arm_l_motor = arm_r_motor = wrist_motor = 127;
}
ramp_up = p4_sw_aux1;
ramp_down = !p4_sw_aux1;
//JAW CONTROL (player 1 top);
grabber = p3_sw_trig | p1_sw_aux2;
//EXTENDER ARM
if (p3_sw_top == 1 && ext_but_prev == 0) {
extender = (extender == 1) ? 0 : 1;
ext_but_prev = 1;
}else{
if (!p3_sw_top) {
ext_but_prev = 0;
}
}
extension = extender;
/*
if (track_a_light) {
if (Targets.num_of_lights == 1) {
desired_robot_angle = desired_robot_angle = ((((int)TILT_SERVO - 127)*65)/127 + Targets.c_light_angle) * 10 + pan_gyro_angle;
}else{
switch (p1_sw_top | (p1_sw_aux1 << 2) | (p1_sw_aux2 << 3)) {
case 1:
desired_robot_angle = desired_robot_angle = ((((int)TILT_SERVO - 127)*65)/127 + Targets.l_light_angle) * 10 + pan_gyro_angle;
break;
case 4:
desired_robot_angle = desired_robot_angle = ((((int)TILT_SERVO - 127)*65)/127 + Targets.c_light_angle) * 10 + pan_gyro_angle;
break;
case 8:
desired_robot_angle = desired_robot_angle = ((((int)TILT_SERVO - 127)*65)/127 + Targets.r_light_angle) * 10 + pan_gyro_angle;
break;
}
}
}*/
//COMPRESSOR CONTROL
//compressor = !pressure_switch;
} /* END Default_Routine(); */
