int32_t shutdown_all(cwiid_wiimote_t *wiimote)
{
stop_motors();
set_ir_led(false);
write_status_led(STATUS_LED_OFF, 0);
return cwiid_close(wiimote);
}
void epuckPlayer()
{
//e_start_agendas_processing();
//e_init_motors();
//e_init_prox();
//e_init_uart1();
/* Must send anything here or it don't work. Is it a bug? */
e_send_uart1_char("epuckSide_v3.0", 14);
unsigned a,b;
/* Flash the LED's in a singular manner for show that this program is in
* epuck memory */
for(a=0; a<8; a++)
for(b=0; b<20000; b++)
e_set_led(a ,1); /* LED ON */
for(a=0; a<8; a++)
for(b=0; b<20000; b++)
e_set_led(a ,0); /* LED OFF */
char command;
while(1)
{
command = recv_char();
switch(command)
{
case 0x13:
recv_vel();
break;
case 0x14:
send_steps();
break;
case 0x16:
read_ir_sensors();
break;
case 0x15:
stop_motors();
break;
case 0x17:
read_camera();
break;
case 0x18:
set_LEDs();
break;
case 0x01:
sendVersion();
break;
case 0x02:
config_camera();
break;
}
}
}
//==============================
//==============================
int main(int argc, char *argv[])
{
int ret;
//printf("[wifibot - go]\n");
/*
if(argc==4)
{
strcpy(host_addr,argv[1]);
target_distance=100*atof(argv[2]); //convert to cm
target_angle=atof(argv[3]);
}
else
{
printf("syntax: go [address] [distance] [angle]\n");
exit(1);
}
*/
if(process_args(argc, argv)<0)
{
printf("Syntax: move -h <address> -d <distance> -a <angle> -s <speed>\n");
exit(1);
}
if((socket_command=socket(AF_INET,SOCK_DGRAM,IPPROTO_UDP))<0)
{
fprintf(stderr,"Command socket error\n");
exit(1);
}
memset((char *) &myaddr_command, 0, sizeof(myaddr_command));
myaddr_command.sin_family=AF_INET;
myaddr_command.sin_port=htons(PORT_COMMAND);
myaddr_command.sin_addr.s_addr=inet_addr(host_addr); //inet_addr(HOST_ADDR);
stop_motors();
if((ret = pthread_create(&threads[0], NULL, th_robot_status, (void *) 0)) != 0)
{
fprintf(stderr,"Status thread (%s)\n", strerror(ret));
exit (1);
}
/*
if((ret = pthread_create(&threads[1], NULL, th_gps, (void *) 0)) != 0)
{
printf("ERRR gps thread (%s)\n", strerror(ret));
exit (1);
}
*/
while(!wait_for_status)
{
usleep(50000);
}
// do the thing and exit
//printf("DIST_START: %.2f\n",target_distance);
me.dist_from_start=0;
rotate();
go_forward();
printf("[wifibot] %.2f\n",me.dist_from_start/100);
return 0;
}
void linecheck(int32_t * u_fwd, int32_t * u_back, uint8_t * iterations, uint8_t moves)
{
if((*iterations) >= moves)
{
(*u_fwd) = 0;
(*u_back) = 0;
(*iterations) = 0;
stop_motors(0); // Change state and stop motors
}
}
void stop_omni_realtime(void)
{
printf("Stopping...\n");
/* Signal a stop the realtime thread */
exiting = 1;
pthread_join(thread, 0);
/* Now stop all motors. */
stop_motors();
printf("Unloading.\n");
}
//===========
void rotate()
{
uchar speed_flag;
target_angle = ((long)target_angle) % 360;
#ifdef debug_printf
printf("ROT: deg %.2f", target_angle);
#endif
target_angle = to_rad(target_angle);
//target_angle = target_angle%(2*M_PI);
if(target_angle>=0)
{
if(target_angle<=M_PI)
{
speed_flag=FLAG_RIGHT;
}
else
{
speed_flag=FLAG_LEFT;
target_angle=2*M_PI-target_angle;
}
}
else
{
target_angle=-target_angle;
if(target_angle<=M_PI)
{
speed_flag=FLAG_LEFT;
}
else
{
speed_flag=FLAG_RIGHT;
target_angle=2*M_PI-target_angle;
}
}
#ifdef debug_printf
printf(" rad %.2f flag %d\n", target_angle, speed_flag);
#endif
if(target_angle>(to_rad(ANGLE_TRESHOLD)))
{
//set_motors_speed_dir(ROTATION_SPEED,ROTATION_SPEED, speed_flag);
while((target_angle-abs(me.angle))>to_rad(ANGLE_OVERFLOW)) //including inertion overturn ~23.3 degrees (speed~70)
{
set_motors_speed_dir(ROTATION_SPEED, ROTATION_SPEED, speed_flag);
usleep(MOTOR_SLEEP);
}
stop_motors();
}
}
void linecheck(int32_t * u_fwd, int32_t * u_back, uint8_t * iterations, uint8_t moves)
{
int32_t temp;
if((*iterations) >= moves)
{
/*ADDED BY ERNIE*/
if (drivestate == DRIVESTATE_FWD) {
//rv_motors(25, 1);
} else {
(*u_fwd) = 0;
(*u_back) = 0;
stop_motors(0); // Change state and stop motors
}
/*END ADDED BY ERNIE*/
(*iterations) = 0;
}
}
//This is the thread that will run with high priority
void* realtimeMain(void* udata)
{
struct timespec tick;
int period = 1e+6; // 1 ms in nanoseconds
while(!exiting)
{
cyclic_task(); // Send and receave from the motor controllers
if(pthread_mutex_trylock(&mutex) == 0)
{
tar = tar_buffer;
cur_buffer = cur;
pthread_mutex_unlock(&mutex);
}
// Compute end of next period
timespecInc(&tick, period);
struct timespec before;
clock_gettime(CLOCK_REALTIME, &before);
if ((before.tv_sec + before.tv_nsec/1e9) > (tick.tv_sec + tick.tv_nsec/1e9))
{
// We overran, snap to next "period"
tick.tv_sec = before.tv_sec;
tick.tv_nsec = (before.tv_nsec / period) * period;
timespecInc(&tick, period);
misses++;
}
// Sleep until end of period
clock_nanosleep(CLOCK_REALTIME, TIMER_ABSTIME, &tick, NULL);
}
stop_motors();
return 0;
}
int main(void)
{
init();
BOOL motors_stopped = FALSE;
send_welcome();
while (1)
{
if (motors_stopped)
{
led_on ( STOP_LED );
led_off ( GO_LED );
} else
{
led_on( GO_LED );
led_off( STOP_LED );
}
read_buttons();
if (stop_button_pressed)
{
motors_stopped = TRUE;
stop_motors();
}
if (prog_button_pressed)
{
motors_stopped = FALSE;
resume_motors();
}
encoder_timeslice();
limit_switch_timeslice();
motor_timeslice_10ms();
serial_timeslice(); // handle incoming messages (if avail)
watchdog_reset();
//delay(one_second/10);
}
return (0);
}
void perform_effect(const int op)
{
switch(op)
{
case MOTOR_1_TURN_R_OPTION:
motor_1_turn_right();
break;
case MOTOR_1_TURN_L_OPTION:
motor_1_turn_left();
break;
case MOTOR_2_TURN_R_OPTION:
motor_2_turn_right();
break;
case MOTOR_2_TURN_L_OPTION:
motor_2_turn_left();
break;
case MOTOR_3_TURN_R_OPTION:
motor_3_turn_right();
break;
case MOTOR_3_TURN_L_OPTION:
motor_3_turn_left();
break;
case MOTORS_1_2_3_TURN_R_OPTION:
motors_1_2_3_turn_right();
break;
case MOTORS_1_2_3_TURN_L_OPTION:
motors_1_2_3_turn_left();
break;
case STOP_MOTORS_OPTION:
stop_motors();
break;
case EXIT_OPTION:
printl("BYE...");
break;
default:
printl("BAD INPUT, Try Again");
break;
}
}
void isr_timer(void)
{
if( qr.link_down )
{
unsigned char i;
qr.current_mode = SAFE_MODE;
stop_motors();
qr.exit = 1;
qr.flag_mode = 1;
for(i=0; i < 5; i++, catnap(500))
X32_LEDS = ALL_ON, catnap(500), X32_LEDS = ALL_OFF;
#ifdef PERIPHERAL_DISPLAY
X32_DISPLAY = 0xf1fa;
#endif
DISABLE_INTERRUPT(INTERRUPT_TIMER1);
}
qr.link_down = 1;
}
/*******************************************************************
* MAIN()
*******************************************************************/
int
main(void)
{
long lEEPROMRetStatus;
uint16_t i=0;
uint8_t halted_latch = 0;
// Set the clocking to run at 80 MHz from the PLL.
// (Well we were at 80MHz with SYSCTL_SYSDIV_2_5 but according to the errata you can't
// write to FLASH at frequencies greater than 50MHz so I slowed it down. I supposed we
// could slow the clock down when writing to FLASH but then we need to find out how long
// it takes for the clock to stabilize. This is on at the bottom of my list of things to do
// for now)
SysCtlClockSet(SYSCTL_SYSDIV_4_5 | SYSCTL_USE_PLL | SYSCTL_XTAL_16MHZ | SYSCTL_OSC_MAIN);
// Initialize the device pinout.
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOA);
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOB);
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOC);
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOD);
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOE);
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOF);
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOG);
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOH);
SysCtlPeripheralEnable(SYSCTL_PERIPH_GPIOJ);
// Enable processor interrupts.
IntMasterEnable();
// Setup the UART's
my_uart_0_init(115200, (UART_CONFIG_WLEN_8 | UART_CONFIG_STOP_ONE | UART_CONFIG_PAR_NONE));
// command_handler_init overwrites the baud rate. We still need to configure the pins though
my_uart_1_init(38400, (UART_CONFIG_WLEN_8 | UART_CONFIG_STOP_ONE | UART_CONFIG_PAR_NONE));
// Enable the command handler
command_handler_init(); // We set the baud in here
// Start the timers
my_timer0_init();
my_timer1_init();
i2c_init();
motor_init();
qei_init();
gyro_init();
accel_init();
led_init();
//rc_radio_init();
//setupBluetooth();
// Initialize the EEPROM emulation region.
lEEPROMRetStatus = SoftEEPROMInit(EEPROM_START_ADDR, EEPROM_END_ADDR, EEPROM_PAGE_SIZE);
if(lEEPROMRetStatus != 0) UART0Send("EEprom ERROR!\n", 14);
#if 0
// If ever we wanted to write some parameters to FLASH without the HMI
// we could do it here.
SoftEEPROMWriteDouble(kP_ID, 10.00);
SoftEEPROMWriteDouble(kI_ID, 10.00);
SoftEEPROMWriteDouble(kD_ID, 10.00);
SoftEEPROMWriteDouble(ANG_ID, 0.0);
SoftEEPROMWriteDouble(COMPC_ID, 0.99);
#endif
kP = SoftEEPROMReadDouble(kP_ID);
kI = SoftEEPROMReadDouble(kI_ID);
kD = SoftEEPROMReadDouble(kD_ID);
commanded_ang = zero_ang = SoftEEPROMReadDouble(ANG_ID);
COMP_C = SoftEEPROMReadDouble(COMPC_ID);
pid_init(kP, kI, kD, &pid_ang);
motor_controller_init(20, 100, 10, &mot_left);
motor_controller_init(20, 100, 10, &mot_right);
//pid_init(0.0, 0.0, 0.0, &pid_pos_left);
//pid_init(0.0, 0.0, 0.0, &pid_pos_right);
//UART0Send("Hello World!\n", 13);
// Tell the HMI what the initial parameters are.
print_params(1);
while(1)
{
delta_t = myTimerValueGet();
myTimerZero();
sum_delta_t += delta_t;
// Read our sensors
accel_get_xyz_cal(&accel_x, &accel_y, &accel_z, true);
gyro_get_y_cal(&gyro_y, false);
// Calculate the pitch angle with the accelerometer only
R = sqrt(pow(accel_x, 2) + pow(accel_z, 2));
accel_pitch_ang = (acos(accel_z / R)*(RAD_TO_DEG)) - 90.0 - zero_ang;
//accel_pitch_ang = (double)((atan2(accel_x, -accel_z))*RAD_TO_DEG - 90.0);
gyro_pitch_ang += (double)gyro_y*g_gyroScale*CONV_TO_SEC(delta_t);
// Kalman filter
//filtered_ang = kalman((double)accel_pitch_ang, ((double)gyro_y)*g_gyroScale, CONV_TO_SEC(delta_t));
filtered_ang = (COMP_C*(filtered_ang+((double)gyro_y*g_gyroScale*CONV_TO_SEC(delta_t)))) + ((1.0-COMP_C)*(double)accel_pitch_ang);
// Skip the rest of the process until the angle stabilizes
if(i < 250) { i++; continue; }
// Tell the HMI what's going on every 100ms
if(sum_delta_t >= 1000)
{
print_update(1);
print_debug(0);
//print_control_surfaces(0);
led_toggle();
//print_angle();
sum_delta_t = 0;
}
// See if the HMI has anything to say
command_handler();
//continue;
// If we are leaning more than +/- FALL_ANG deg off center it's hopeless.
// Turn off the motors in hopes of some damage control
if( abs(filtered_ang) > FALL_ANG )
{
if(halted_latch) continue;
stop_motors();
halted_latch = 1;
continue;
}
halted_latch = 0;
motor_val = pid_controller(calc_commanded_angle(0), filtered_ang, delta_t, &pid_ang);
motor_left = motor_right = motor_val;
drive_motors(motor_left*left_mot_gain, motor_right*right_mot_gain);
}
}
int main(void)
{
//Sväng inte
turn = 0;
//Initiera spi, pwm och display
spi_init();
pwm_init();
init_display();
update();
claw_out();
_delay_ms(500);
claw_in();
//Aktivera global interrupts
sei();
//Initiera regulator
clear_pid();
init_pid(80, -80);
//update_k_values(40, 12, 22);
update_k_values(40, 170, 20);
// Pekare till aktuell position i bufferten
tmp_sensor_buffer_p = 0x00;
// Flagga som avgör huruvida vi är i början av meddelande
sensor_start = 1;
// Anger aktuell längd av meddelandet
tmp_sensor_buffer_len = 0x00;
//Initiera standardsträng på display
init_default_printf_string();
clear_screen();
update();
while(1)
{
uint8_t has_comm_data, has_sensor_data, comm_data, sensor_data;
do_spi(&has_comm_data, &has_sensor_data, &comm_data, &sensor_data);
//Undersök och hantera meddelanden från slavarna
if(has_comm_data) decode_comm(comm_data);
if(has_sensor_data) decode_sensor(sensor_data);
//Vid manuell sväng eller 180 grader måste make_turn anropas
if(!autonomous || turning_180)
{
if(turn_dir)
{
make_turn_flag = 1;
make_turn(turn_dir);
if(!make_turn_flag)
{
turn_dir = 0;
stop_motors();
}
}
}
//Kör regulatorn
if (regulator_enable)
{
regulator(sensor_buffer[IR_RIGHT_FRONT] - sensor_buffer[IR_RIGHT_BACK],
sensor_buffer[IR_LEFT_FRONT] - sensor_buffer[IR_LEFT_BACK],
sensor_buffer[IR_RIGHT_FRONT] - sensor_buffer[IR_LEFT_FRONT],
sensor_buffer[IR_RIGHT_BACK] - sensor_buffer[IR_LEFT_BACK]);
regulator_enable = 0;
}
}
}
int main(int argc, char* argv[])
{
ros::init(argc, argv, "phidgets_motor_control_hc");
ros::NodeHandle n;
ros::NodeHandle nh("~");
int serial_number = -1;
nh.getParam("serial", serial_number);
std::string name = "motorcontrol";
nh.getParam("name", name);
nh.getParam("x_forward", x_forward);
nh.getParam("rotation", rotation_offset);
std::string odometry_topic = "odom";
nh.getParam("odometry", odometry_topic);
double g = 0;
const double min_g = 0.00001;
nh.getParam("max_angular_error", g);
if (g>min_g) max_angular_error = g;
g=0;
nh.getParam("max_velocity_error", g);
if (g>min_g) max_velocity_error = g;
g=0;
nh.getParam("linear_deadband", g);
if (g>min_g) linear_deadband = g;
g=0;
nh.getParam("angular_deadband", g);
if (g>min_g) angular_deadband = g;
g=0;
nh.getParam("linear_proportional", g);
if (g>min_g) linear_velocity_proportional = g;
g=0;
nh.getParam("linear_integral", g);
if (g>min_g) linear_velocity_integral = g;
g=0;
nh.getParam("linear_derivative", g);
if (g>min_g) linear_velocity_derivative = g;
g=0;
nh.getParam("angular_proportional", g);
if (g>min_g) angular_velocity_proportional = g;
g=0;
nh.getParam("angular_integral", g);
if (g>min_g) angular_velocity_integral = g;
g=0;
nh.getParam("angular_derivative", g);
if (g>min_g) angular_velocity_derivative = g;
g=0;
nh.getParam("max_angular_velocity", g);
if (g>min_g) max_angular_velocity = g;
nh.getParam("invert_rotation", invert_rotation);
nh.getParam("invert_forward", invert_forward);
if (serial_number==-1) {
nh.getParam("serial_number", serial_number);
}
g=0;
nh.getParam("max_angular_accel", g);
if (g>0) max_angular_accel = g;
g=0;
nh.getParam("max_linear_accel", g);
if (g>0) max_linear_accel = g;
std::string topic_path = "phidgets/";
nh.getParam("topic_path", topic_path);
int timeout_sec = 2;
nh.getParam("timeout", timeout_sec);
int v=0;
nh.getParam("speed", v);
if (v>0) speed = v;
int frequency = 30;
nh.getParam("frequency", frequency);
ITerm[0]=0;
ITerm[1]=0;
if (attach(phid, serial_number)) {
display_properties(phid);
const int buffer_length = 100;
std::string topic_name = topic_path + name;
std::string service_name = name;
if (serial_number > -1) {
char ser[10];
sprintf(ser,"%d", serial_number);
topic_name += "/";
topic_name += ser;
service_name += "/";
service_name += ser;
}
motors_pub =
n.advertise<ros_phidgets_jade::motor_params>(topic_name,
buffer_length);
// receive velocity commands
ros::Subscriber command_velocity_sub =
n.subscribe("cmd_vel", 1, velocityCommandCallback);
// subscribe to odometry
ros::Subscriber odometry_sub =
n.subscribe(odometry_topic, 1, odometryCallback);
initialised = true;
ros::Rate loop_rate(frequency);
while (ros::ok()) {
ros::spinOnce();
loop_rate.sleep();
// SAFETY FEATURE
// if a velocity command has not been received
// for a period of time then stop the motors
double time_since_last_command_sec =
(ros::Time::now() -
last_velocity_command).toSec();
if ((motors_active) &&
(time_since_last_command_sec > timeout_sec)) {
stop_motors();
ROS_WARN("No velocity command received - " \
"motors stopped");
}
}
disconnect(phid);
}
return 0;
}
void update_field(){
copy_objects();
int current_x = (int)(game.coords[0].x * VPS_RATIO);
int current_y = (int)(game.coords[0].y * VPS_RATIO);
uint8_t has_encoder_moved = FALSE;
/*if(get_time() > field_state.last_encoder_update + ENCODER_UPDATE_TIME){
if(encoder_read(FREEWHEEL_ENCODER_PORT) != field_state.encoder_value){
field_state.robot_stopped = TRUE;
field_state.encoder_value = encoder_read(FREEWHEEL_ENCODER_PORT);
field_state.
}
else{
field_state.robot_stopped = TRUE;
}
}*/
//printf("Current X: %d, Current Y: %d\n", current_x, current_y);
float current_theta = (((float)game.coords[0].theta) * 180.0) / 2048;
field_state.curr_loc_plus_delta.x = current_x; //+ get_delta(field_state.curr_loc).x;
field_state.curr_loc_plus_delta.y = current_y; //+ get_delta(field_state.curr_loc).y;
uint8_t sextant = get_current_sextant(field_state.curr_loc_plus_delta);
uint8_t old_sextant = get_current_sextant(field_state.curr_loc);
//printf("Current Distance: %f, Waypoint Distance: %f, Target Distance: %f, Distance Increment: %f\n", pythagorean(field_state.curr_loc_plus_delta.x, field_state.curr_loc_plus_delta.y), pythagorean(field_state.target_loc_waypoint.x, field_state.target_loc_waypoint.y), pythagorean(field_state.target_loc.x, field_state.target_loc.y), field_state.distance_increment);
float dist_to_waypoint = pythagorean_loc(field_state.curr_loc_plus_delta, field_state.target_loc_waypoint);
if(sextant != old_sextant ){//|| (get_angle_error_circle() > 90 && dist_to_waypoint < CIRCLE_DECELERATE_DISTANCE_SECOND){
uint8_t target_sextant = get_current_sextant(field_state.target_loc);
/*int direction;
if((get_angle_error_circle() > 90 && dist_to_waypoint < CIRCLE_DECELERATE_DISTANCE_SECOND) && (sextant == old_sextant)){
//we're about to spiral...let's not spiral
direction = get_waypoint_direction(field_state.curr_loc_plus_delta, field_state.target_loc, gyro_get_degrees());
if(get_sextant_difference(field_state.curr_loc_plus_delta, field_state.target_loc, direction) > 1){
//we aren't at the target sextant yet, so set target waypoint to next waypoint
float distance_increment;
switch(direction){
case COUNTER_CLOCKWISE:
if(target_sextant < sextant)
target_sextant += 6;
new_distance = bound_waypoint_distance(field_state.distance_increment, field_state.target_loc_waypoint);
field_state.target_loc_waypoint = get_scaled_loc_int_param(bisecting_points[(2*sextant+4)%12], bisecting_points[(2*sextant+5)%12], new_distance);
break;
case CLOCKWISE:
if(target_sextant > sextant)
target_sextant -= 6;
new_distance = bound_waypoint_distance(field_state.distance_increment, field_state.target_loc_waypoint);
field_state.target_loc_waypoint = get_scaled_loc_int_param(bisecting_points[mod_ui(2*sextant - 2, 12)], bisecting_points[mod_ui(2*sextant - 1, 12)], new_distance);
break;
}
}
}
}*/
//We have broken the plane, get next waypoint, or target if in same sextant
int direction = get_waypoint_direction(field_state.curr_loc_plus_delta, field_state.target_loc, gyro_get_degrees());
if(sextant != target_sextant){
float distance_increment;
if(sextant == mod_ui(old_sextant + direction, 6)){
float new_distance;
switch(direction){
case COUNTER_CLOCKWISE:
if(target_sextant < sextant)
target_sextant += 6;
new_distance = bound_waypoint_distance(field_state.distance_increment, field_state.target_loc_waypoint);
field_state.target_loc_waypoint = get_scaled_loc_int_param(bisecting_points[(2*sextant+2)%12], bisecting_points[(2*sextant+3)%12], new_distance);
break;
case CLOCKWISE:
if(target_sextant > sextant)
target_sextant -= 6;
new_distance = bound_waypoint_distance(field_state.distance_increment, field_state.target_loc_waypoint);
field_state.target_loc_waypoint = get_scaled_loc_int_param(bisecting_points[(2*sextant)%12], bisecting_points[(2*sextant + 1)%12], new_distance);
break;
}
}
field_state.target_loc_plane = get_scaled_loc(field_state.target_loc_waypoint, OUTER_HEX_APATHEM_DIST);
}
else{
field_state.target_loc_waypoint = field_state.target_loc;
}
field_state.curr_loc = field_state.curr_loc_plus_delta;
encoder_reset(LEFT_ENCODER_PORT);
encoder_reset(RIGHT_ENCODER_PORT);
}
if((field_state.substage == TERRITORY_APPROACH_SUBSTAGE) && current_vel > 0){
if(get_time() - field_state.stored_time > TERRITORY_TIMEOUT_TIME || game.coords[0].score != field_state.score){
if(!(field_state.we_want_to_dump)){
field_state.substage = DRIVE_SUBSTAGE;
//SET MOTORS TO LEVER ARC THINGY
target_vel = 96;
current_vel = 96;
set_spinners(0);
//accelerate_time = CIRCLE_ACCELERATE_TIME;
//decelerate_distance = CIRCLE_DECELERATE_DISTANCE;
//KP_CIRCLE = 1.5;
//set_motors(55, 120);
field_state.stored_time = get_time();
field_state.drive_direction = FORWARD;
field_state.pid_enabled = TRUE;
field_state.circle_PID.enabled = true;
servo_set_pos(1, SERVO_UP);
set_new_destination(field_state.curr_loc_plus_delta, get_lever_pivot_point_loc(sextant));
}
else{
retreat_from_territory();
}
}
if(get_time() - field_state.start_time > THINK_ABOUT_DUMPING_TIME){
field_state.we_want_to_dump = TRUE;
}
}
if((field_state.substage == LEVER_APPROACH_SUBSTAGE) && current_vel > 0){
if(get_time() - field_state.stored_time > LEVER_APPROACH_TIME){
field_state.substage = LEVER_SUBSTAGE;
//SET MOTORS TO LEVER ARC THINGY
//target_vel = 64;
//current_vel = 64;
//set_spinners(0);
//accelerate_time = CIRCLE_ACCELERATE_TIME;
//decelerate_distance = CIRCLE_DECELERATE_DISTANCE;
//KP_CIRCLE = 1.5;
//set_motors(55, 120);
stop_motors();
field_state.stored_time = get_time();
field_state.target_loc_waypoint = field_state.curr_loc_plus_delta;
field_state.target_loc = field_state.curr_loc_plus_delta;
field_state.pid_enabled = TRUE;
field_state.stored_time = get_time();
int score_temp = field_state.score;
servo_set_pos(1, SERVO_DOWN);
pause(CLICKY_CLICKY_TIME + 100);
while(1){
copy_objects();
if(get_time() - field_state.start_time > THINK_ABOUT_DUMPING_TIME){
field_state.we_want_to_dump = TRUE;
}
if(game.coords[0].score != score_temp){
score_temp += 40;
field_state.balls_held += 1;
}
if(game.coords[0].score == field_state.score + 200 || field_state.stored_time + LEVER_TIMEOUT_TIME < get_time() ){
if(!field_state.we_want_to_dump){
pause(50);
field_state.substage = DRIVE_SUBSTAGE;
target_vel = TARGET_CIRCLE_VEL;
current_vel = 0;
accelerate_time = CIRCLE_ACCELERATE_TIME;
decelerate_distance = CIRCLE_DECELERATE_DISTANCE_FIRST;
KP_CIRCLE = KP_DRIVE;
field_state.stored_time = get_time();
field_state.start_drive_time = get_time();
field_state.drive_direction = BACKWARD;
field_state.circle_PID.enabled = true;
field_state.pid_enabled = true;
uint8_t temp_sextant = sextant;
if(temp_sextant == 0)
temp_sextant += 6;
set_new_destination(field_state.curr_loc_plus_delta, get_territory_pivot_point_loc(get_target_territory(field_state.curr_loc_plus_delta)));
break;
}
else{
pause(50);
get_your_ass_to_a_toilet();
break;
}
}
servo_set_pos(1, SERVO_MIDDLE);
pause(CLICKY_CLICKY_TIME);
servo_set_pos(1, SERVO_DOWN);
pause(CLICKY_CLICKY_TIME);
}
}
}
/*if(field_state.substage == TRANSITION_SUBSTAGE){
if(get_time() - field_state.stored_time > TRANSITION_TIME){
field_state.substage = LEVER_SUBSTAGE;
stop_motors();
field_state.stored_time = get_time();
field_state.target_loc_waypoint = field_state.curr_loc_plus_delta;
field_state.target_loc = field_state.curr_loc_plus_delta;
field_state.pid_enabled = TRUE;
field_state.circle_PID.enabled = true;
while(1){
copy_objects();
if(game.coords[0].score == field_state.score + 200 || field_state.stored_time + LEVER_TIMEOUT_TIME < get_time() ){
field_state.substage = DRIVE_SUBSTAGE;
target_vel = 128;
current_vel = 0;
accelerate_time = CIRCLE_ACCELERATE_TIME;
decelerate_distance = CIRCLE_DECELERATE_DISTANCE_FIRST;
KP_CIRCLE = KP_DRIVE;
field_state.stored_time = get_time();
field_state.start_drive_time = get_time();
field_state.drive_direction = BACKWARD;
field_state.circle_PID.enabled = true;
uint8_t temp_sextant = sextant;
if(temp_sextant == 0)
temp_sextant += 6;
set_new_destination(field_state.curr_loc_plus_delta, get_territory_pivot_point_loc((temp_sextant - 1)%6));
break;
}
servo_set_pos(1, SERVO_DOWN);
pause(CLICKY_CLICKY_TIME);
servo_set_pos(1, SERVO_MIDDLE);
pause(CLICKY_CLICKY_TIME);
}
}
}*/
/*else if((field_state.substage == LEVER_APPROACH_SUBSTAGE) && current_vel > 0){
if(get_time() - field_state.stored_time > TERRITORY_TIMEOUT_TIME || game.coords[0].score != field_state.score){
field_state.substage = LEVER_APPROACH_SUBSTAGE;
target_vel = 64;
current_vel = 0;
set_spinners(0);
//accelerate_time = CIRCLE_ACCELERATE_TIME;
//decelerate_distance = CIRCLE_DECELERATE_DISTANCE;
//KP_CIRCLE = 1.5;
//field_state.start_drive_time = get_time();
field_state.drive_direction = FORWARD;
set_new_destination(field_state.curr_loc_plus_delta, get_lever_robot_loc((sextant)%6));
}
}*/
if(get_time() - field_state.start_time > THINK_ABOUT_DUMPING_TIME){
if(field_state.substage == DRIVE_SUBSTAGE){
if(!field_state.we_want_to_dump)
get_your_ass_to_a_toilet();
}
field_state.we_want_to_dump = TRUE;
}
int acceptable_error = get_acceptable_error(field_state.substage);
float dist_to_target = pythagorean(field_state.target_loc.x - field_state.curr_loc_plus_delta.x, field_state.target_loc.y - field_state.curr_loc_plus_delta.y);
if(field_state.target_loc.x == field_state.target_loc_waypoint.x && field_state.target_loc.y == field_state.target_loc_waypoint.y){
if(dist_to_target < acceptable_error){
gyro_set_degrees(current_theta);
encoder_reset(LEFT_ENCODER_PORT);
encoder_reset(RIGHT_ENCODER_PORT);
/*if(field_state.substage == TERRITORY_APPROACH_SUBSTAGE){
field_state.substage = TERRITORY_SUBSTAGE;
stop_motors();
set_spinners(0);
current_vel = 0;
target_vel = 0;
}
if(field_state.substage == LEVER_APPROACH_SUBSTAGE){
field_state.substage = LEVER_SUBSTAGE;
stop_motors();
current_vel = 0;
target_vel = 0;
}*/
if(field_state.substage == DRIVE_SUBSTAGE || field_state.substage == TERRITORY_RETREAT_SUBSTAGE || field_state.substage == LEVER_RETREAT_SUBSTAGE){
if(field_state.stage == FIRST_STAGE)
field_state.stage = SECOND_STAGE;
if(field_state.target_loc.x == get_territory_pivot_point_loc(sextant).x && field_state.target_loc.y == get_territory_pivot_point_loc(sextant).y){
if(field_state.substage != TERRITORY_RETREAT_SUBSTAGE){
set_new_destination(field_state.curr_loc_plus_delta, get_territory_robot_loc(sextant));
set_spinners( 229 * field_state.color);
target_vel = 64;
current_vel = 0;
field_state.start_drive_time = get_time();
accelerate_time = WALL_ACCELERATE_TIME;
decelerate_distance = (int)(WALL_DECELERATE_DISTANCE);
field_state.substage = TERRITORY_APPROACH_SUBSTAGE;
KP_CIRCLE = KP_APPROACH;
field_state.drive_direction = BACKWARD;
}
else{
field_state.substage = PIVOT_SUBSTAGE;
uint8_t dump_sextant = pick_dump_sextant();
Location dump_loc = get_dump_location_robot(dump_sextant);
set_new_destination(field_state.curr_loc_plus_delta, dump_loc);
current_vel = 0;
}
}
else if(field_state.target_loc.x == get_lever_pivot_point_loc(sextant).x && field_state.target_loc.y == get_lever_pivot_point_loc(sextant).y){
if(field_state.substage != LEVER_RETREAT_SUBSTAGE){
target_vel = 64;
current_vel = 0;
set_new_destination(field_state.curr_loc_plus_delta, get_lever_robot_loc(sextant));
decelerate_distance = (int)(WALL_DECELERATE_DISTANCE);
field_state.start_drive_time = get_time();
accelerate_time = WALL_ACCELERATE_TIME;
KP_CIRCLE = KP_APPROACH;
field_state.drive_direction = FORWARD;
field_state.substage = LEVER_APPROACH_SUBSTAGE;
}
else{
field_state.substage = PIVOT_SUBSTAGE;
//WE WILL PROBABLY NEVER USE LEVER_RETREAT_SUBSTAGE...
}
}
else if(field_state.target_loc.x == get_dump_location_robot(sextant).x && field_state.target_loc.y == get_dump_location_robot(sextant).y){
field_state.substage = DUMPING_SUBSTAGE;
set_new_destination(field_state.curr_loc_plus_delta, get_dump_location(sextant));
current_vel = 0;
target_vel = 64;
field_state.start_dump_time = get_time();
decelerate_distance = 0;
KP_CIRCLE = KP_APPROACH;
field_state.drive_direction = BACKWARD;
}
}
}
}
run_pivot_subroutine();
if(field_state.substage == DUMPING_SUBSTAGE){
if(get_time() - field_state.start_dump_time > 4000)
servo_set_pos(0, 150);
if(get_time() - field_state.start_dump_time > 4000 + time_during_dump){
servo_set_pos(0, 300);
}
}
if(current_x != field_state.curr_loc.x || current_y != field_state.curr_loc.y || current_theta != field_state.curr_angle){
field_state.update_time = get_time();
field_state.curr_loc.x = current_x;
field_state.curr_loc.y = current_y;
field_state.curr_angle = current_theta;
//printf("Lag: %lu, CX: %d, CY: %d, OX: %d, OY: %d, TX: %d, TY: %d, TWX: %d, TWY: %d, CA: %.2f, G: %.3f, LE: %d, RE: %d\n", get_time() - field_state.stored_time, current_x, current_y, 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));
//field_state.stored_time = get_time();
encoder_reset(LEFT_ENCODER_PORT);
encoder_reset(RIGHT_ENCODER_PORT);
if(field_state.substage == DRIVE_SUBSTAGE)
gyro_set_degrees(current_theta);
}
field_state.curr_time = get_time();
//***TIMEOUTS***
/*if(field_state.curr_time - field_state.update_time >= DRIVE_TIMEOUT_TIME && !is_decelerating() && field_state.substage == DRIVE_SUBSTAGE && dist_to_target < acceptable_error){
//We hit a wall...or another robot, but we haven't coded robot-ramming timeouts yet...
float distance = pythagorean(field_state.curr_loc_plus_delta.x, field_state.curr_loc_plus_delta.y);
if(fabs(distance - get_min_distance_loc_param(field_state.curr_loc)) < fabs(distance - get_max_distance_loc_param(field_state.curr_loc))){
//We hit the inner hex
}
}*/
field_state.score = game.coords[0].score;
}
ErrorID_t acceleration_mode(cwiid_wiimote_t *wiimote,
volatile WiimoteStatusDataType *wiimote_status)
{
typedef enum WiimoteAccelStateType
{
WIIMOTE_ACCEL_WAIT_FOR_START,
WIIMOTE_ACCEL_READ_CAL_DATA,
WIIMOTE_ACCEL_READ_DATA,
WIIMOTE_ACCEL_WAIT_FOR_EXIT,
WIIMOTE_ACCEL_EXIT,
} WiimoteAccelStateType;
const uint8_t RETRY_VALUE = 2;
uint8_t retry_count = 0;
ErrorID_t error_flag = ERR_NONE;
WiimoteAccelStateType state = WIIMOTE_ACCEL_WAIT_FOR_START;
debug_print("Entering acceleration mode...\n");
write_status_led(STATUS_LED_OFF, 0);
for (;;)
{
switch (state)
{
case WIIMOTE_ACCEL_WAIT_FOR_START:
if (0 > wait_for_wiimotedata(wiimote_status, INFINITE_TIMEOUT))
{
debug_print("No start signal received\n");
error_flag = WII_ERROR_DATA_TIMEOUT;
state = WIIMOTE_ACCEL_EXIT;
}
else if (0 == (wiimote_status->button_data & CWIID_BTN_A))
{
state = WIIMOTE_ACCEL_READ_CAL_DATA;
}
break;
case WIIMOTE_ACCEL_READ_CAL_DATA:
error_flag = WII_ERROR_ACCEL_CAL;
do
{
if (0
== cwiid_get_acc_cal(
wiimote,
CWIID_EXT_NONE,
(struct acc_cal *) &wiimote_status->accel_cal_data))
{
error_flag = ERR_NONE;
break;
}
} while (retry_count++ < RETRY_VALUE);
if (error_flag != ERR_NONE)
{
debug_print("Cannot read cal data\n");
return error_flag;
}
if (0 > cwiid_set_rpt_mode(wiimote, CWIID_RPT_ACC | CWIID_RPT_BTN))
{
debug_print("Cannot set report mode to ACCEL\n");
return false;
}
set_lcd(0, "Accel Mode...");
set_lcd(1, "P = %4d R = %4d", 0, 0);
state = WIIMOTE_ACCEL_READ_DATA;
break;
case WIIMOTE_ACCEL_READ_DATA:
if (0 > wait_for_wiimotedata(wiimote_status, INFINITE_TIMEOUT))
{
debug_print("@%u: WII_ERROR_DATA_TIMEOUT\n", get_tick_count());
error_flag = WII_ERROR_DATA_TIMEOUT;
state = WIIMOTE_ACCEL_EXIT;
}
else
{
if (wiimote_status->button_data & CWIID_BTN_A)
{
state = WIIMOTE_ACCEL_WAIT_FOR_EXIT;
}
else
{
error_flag = computer_motor_levels_accel(wiimote_status);
set_lcd(1, "P = %4d R = %4d",
wiimote_status->accel_computed_data.pitch / 100,
wiimote_status->accel_computed_data.roll / 100);
if (ERR_EXEC == error_flag)
cwiid_set_rumble(wiimote, true);
else
{
cwiid_set_rumble(wiimote, false);
if (error_flag < 0)
{
debug_print("@%u: CONTROL CAR COMM ERROR: %d\n",
get_tick_count(), error_flag);
state = WIIMOTE_ACCEL_EXIT;
}
}
}
}
break;
case WIIMOTE_ACCEL_WAIT_FOR_EXIT:
stop_motors();
if (0 < wait_for_wiimotedata(wiimote_status, INFINITE_TIMEOUT))
{
error_flag = WII_ERROR_DATA_TIMEOUT;
state = WIIMOTE_ACCEL_EXIT;
}
else if (0 == (wiimote_status->button_data & CWIID_BTN_A))
{
state = WIIMOTE_ACCEL_EXIT;
}
break;
case WIIMOTE_ACCEL_EXIT:
debug_print("Exiting acceleration mode.\n\n");
return error_flag;
}
}
return ERR_NONE;
}
void move()
{
int i;
/*variable to use in figuring out the "best" option*/
int max_q_score = 0;
/*what do we do next? store it here*/
/*we init to -1 as an error*/
int next_movement = -1;
/*Where we started.*/
/*We don't use ROTATION_2 all the way through in case it changes.*/
int initial_angle = norm_rotation(ROTATION_2);
/*Where we ended up.*/
int new_angle;
/*Show the current angle*/
cputc_native_user(CHAR_A, CHAR_N, CHAR_G, CHAR_L); // ANGL
msleep(200);
lcd_int(initial_angle);
msleep(500);
/*
* Most of the time, we do the "correct" thing
* by finding the best q_score of our possible options.
* On the off chance that norm_random() is low (or EPSILON is high ;)
* we then "explore" by choosing a random movement.
*/
if(norm_random() > EPSILON_CURRENT)
{
/*We are doing what the table tells us to.*/
cputc_native_user(CHAR_r, CHAR_e, CHAR_a, CHAR_l); // real
msleep(500);
for(i=0; i<MOVEMENTS; i++)
{
if(q_score[initial_angle][i] > max_q_score)
{
max_q_score = q_score[initial_angle][i];
next_movement = i;
}
}
}
else
{
double temp;
/*We are just picking something at random.*/
cputc_native_user(CHAR_r, CHAR_a, CHAR_n, CHAR_d); // rand
msleep(500);
/*pick one. Any one.*/
temp = norm_random();
next_movement = temp*MOVEMENTS;
/*show what we do next*/
lcd_int(next_movement);
sleep(1);
}
/*what happens if next_movement never gets changed?*/
/*we'd hate to do HARD_LEFT over and over again*/
/*so we choose randomly*/
if(-1==next_movement)
{
double temp;
temp = norm_random();
next_movement = temp*MOVEMENTS;
}
/*having chosen a movement, lets do it*/
switch(next_movement)
{
case HARD_LEFT:
cputc_native_user(CHAR_H, CHAR_L, 0, 0); // HL
hard_left();
break;
case SOFT_LEFT:
cputc_native_user(CHAR_S, CHAR_L, 0, 0); // SL
soft_left();
break;
case FORWARD:
cputc_native_user(CHAR_F, CHAR_W, CHAR_W, CHAR_D); // FWD
go_forward();
break;
case SOFT_RIGHT:
cputc_native_user(CHAR_S, CHAR_R, 0, 0); // SR
soft_right();
break;
case HARD_RIGHT:
cputc_native_user(CHAR_H, CHAR_R, 0, 0); // HR
hard_right();
break;
case REVERSE:
cputc_native_user(CHAR_R, CHAR_E, CHAR_V, 0); // REV
go_back();
break;
default:
/*this is an error and should never be reached*/
cputc_native_user(CHAR_E, CHAR_R, CHAR_R, 0); // ERR
stop_motors();
sleep(1);
break;
}
/*Once we've started, we'd better stop*/
stop_motors();
/*Allows us to read direction*/
msleep(500);
/*This is here just to make the next function cleaner*/
new_angle = norm_rotation(ROTATION_2);
/*Where are we now?*/
cputc_native_user(CHAR_N, CHAR_E, CHAR_W, CHAR_W); // NEW
msleep(200);
lcd_int(new_angle);
msleep(500);
/*
* Since we know that "next_movement" took us from "initial_angle"
* to new_angle (ROTATION_2), we store that increased probability.
*/
steering_results[initial_angle][next_movement][new_angle] += ALPHA;
/*here we re-norm so that the sum of the probabilities is still 1*/
for(i=0; i<ANGLES; i++)
{
steering_results[initial_angle][next_movement][i] /= (1+ALPHA);
}
/*The last thing we do is reduce Epsilon*/
if(EPSILON_CURRENT > EPSILON_MIN)
{
EPSILON_CURRENT-=EPSILON_DECAY;
}
}
//===============
void go_forward()
{
float angle_diff;
uchar lspeed, rspeed, speed_flag, current_speed=0;
#ifdef debug_printf
printf("MOV: %f\n",target_distance);
#endif
if(target_distance>=0)
{
speed_flag=FLAG_FORWARD;
}
else
{
target_distance=-target_distance;
speed_flag=FLAG_BACKWARD;
}
if(target_distance>INERTION)
{
while(moving_allowed&&(me.dist_from_start<(target_distance-INERTION)))
{
//acceleration
if(current_speed<input_speed)
{
current_speed += ACCELERATION;
if(current_speed>input_speed)
current_speed = input_speed;
lspeed=rspeed=current_speed;
}
else
{
lspeed=rspeed=input_speed;
// speed-trajectory correction
angle_diff = target_angle-me.angle;
if(angle_diff>to_rad(ANGLE_DIFF))
{
if(speed_flag==FLAG_FORWARD)
{
lspeed += 5; //lspeed += (2*ANGLE_CORRECTION*angle_diff)/M_PI;
rspeed -= 15; //rspeed -= (2*ANGLE_CORRECTION*angle_diff)/M_PI;
}
if(speed_flag==FLAG_BACKWARD)
{
rspeed += 5;
lspeed -= 15;
}
}
if(angle_diff<-to_rad(ANGLE_DIFF))
{
if(speed_flag==FLAG_FORWARD)
{
rspeed += 5; //rspeed += (2*ANGLE_CORRECTION*angle_diff)/M_PI;
lspeed -= 15; //lspeed -= (2*ANGLE_CORRECTION*angle_diff)/M_PI;
}
if(speed_flag==FLAG_BACKWARD)
{
lspeed += 5;
rspeed -= 15;
}
}
}
set_motors_speed_dir(lspeed, rspeed, speed_flag);
usleep(MOTOR_SLEEP);
}
// decceleration
while(moving_allowed&&(current_speed>0))
{
current_speed -= ACCELERATION<<1;
if(current_speed<0)
current_speed = 0;
lspeed = rspeed = current_speed;
}
stop_motors();
}
}
/*!
@brief Determines motor parameters from IR data.
*/
ErrorID_t infrared_mode(cwiid_wiimote_t *wiimote,
volatile WiimoteStatusDataType *wiimote_status)
{
typedef enum WiimoteInfraredStateType
{
WIIMOTE_INFRARED_WAIT_FOR_START,
WIIMOTE_INFRARED_ENABLE,
WIIMOTE_INFRARED_READ_DATA,
WIIMOTE_INFRARED_WAIT_FOR_EXIT,
WIIMOTE_INFRARED_EXIT,
} WiimoteInfraredStateType;
ErrorID_t error_flag = ERR_NONE;
bool last_valid = false;
bool valid_points = false;
WiimoteInfraredStateType state = WIIMOTE_INFRARED_WAIT_FOR_START;
write_status_led(STATUS_LED_OFF, 0);
for (;;)
{
switch (state)
{
case WIIMOTE_INFRARED_WAIT_FOR_START:
if (0 < wait_for_wiimotedata(wiimote_status, 500))
{
error_flag = WII_ERROR_DATA_TIMEOUT;
state = WIIMOTE_INFRARED_EXIT;
}
else if (0 == (wiimote_status->button_data & CWIID_BTN_B))
{
state = WIIMOTE_INFRARED_ENABLE;
}
break;
case WIIMOTE_INFRARED_ENABLE:
set_ir_led(STATUS_LED_ON);
debug_print("Setting IR Report\n");
if (0 < cwiid_set_rpt_mode(wiimote, CWIID_RPT_IR | CWIID_RPT_BTN))
{
debug_print("Cannot set report mode to IR\n");
return false;
}
set_lcd(0, "Infrared Mode");
set_lcd(1, "Point wiimote at car");
write_status_led(STATUS_LED_ON, 0);
state = WIIMOTE_INFRARED_READ_DATA;
break;
case WIIMOTE_INFRARED_READ_DATA:
if (0 < wait_for_wiimotedata(wiimote_status, 500))
{
error_flag = WII_ERROR_DATA_TIMEOUT;
state = WIIMOTE_INFRARED_EXIT;
}
else if (wiimote_status->button_data & CWIID_BTN_B)
{
state = WIIMOTE_INFRARED_WAIT_FOR_EXIT;
}
else
{
error_flag = WiiComputeMotorLevelsInfrared(wiimote_status,
&valid_points);
if (valid_points != last_valid)
{
write_status_led(
valid_points ? STATUS_LED_OFF : STATUS_LED_ON, 0);
set_lcd(
1,
valid_points ?
"LED's seen" : "Point wiimote at car");
}
last_valid = valid_points;
if (ERR_EXEC == error_flag)
cwiid_set_rumble(wiimote, true);
else
{
cwiid_set_rumble(wiimote, false);
if (error_flag < 0)
state = WIIMOTE_INFRARED_EXIT;
}
}
break;
case WIIMOTE_INFRARED_WAIT_FOR_EXIT:
if (0 < wait_for_wiimotedata(wiimote_status, 500))
{
state = WIIMOTE_INFRARED_EXIT;
}
else if (0 == (wiimote_status->button_data & CWIID_BTN_B))
{
state = WIIMOTE_INFRARED_EXIT;
}
break;
case WIIMOTE_INFRARED_EXIT:
stop_motors();
write_status_led(STATUS_LED_OFF, 0);
set_ir_led(false);
return error_flag;
break;
break;
}
}
return true;
}
