void change_state_to(unsigned char new_state){
// clear all the relevant timers?
debug_all_off();
stop_moving();
beacon_sensing_state_changed();
motor_state_changed(); // resets the pulse clock?
current_state = new_state;
just_entered_state = true;
state_changed = true;
state_init_finished = false;
Serial.println(" "); // take this off later
}
void check_pulse(){
if (is_pulsing){
// Serial.println("Pulsing");
} else {
// Serial.println("not pulsing");
}
if (is_pulsing && pulse_timer_finished()){
stop_moving();
is_pulsing = false;
start_pulse_timer(current_pulse_delay); // delay timer
}
if (!is_pulsing && pulse_timer_finished()){
is_pulsing = true;
pulse_fn();
}
}
int main(void)
{
uint8_t retval;
while (1) {
init();
for (retval = 0; retval < 50; retval++) {
/* wait x ms until inputs are debounced */
wait_for_timer_tick();
}
if (direction == ASK) {
if (position() == OPEN) direction = DOWN;
else direction = UP;
}
if (!get_key_press(START_BT)) {
goto unintended_wakeup;
}
led1(GREEN);
start_moving(direction);
retval = 1;
while (run_enable && retval) {
wait_for_timer_tick();
retval = pwm_ramp_up();
if (get_key_press(START_BT)) {
disable();
}
}
while (run_enable) {
wait_for_timer_tick();
if (get_key_press(START_BT)) {
disable();
}
}
retval = 1;
while (retval) {
wait_for_timer_tick();
retval = pwm_ramp_down();
}
stop_moving();
unintended_wakeup:
led1(OFF);
deinit();
sleep();
}
/* never reached */
return 0;
}
vector<Vector3d> MeanShift::cluster(vector<Vector3d> points, double kernel_bandwidth){
vector<bool> stop_moving(points.size(), false);
stop_moving.reserve(points.size());
vector<Vector3d> shifted_points = points;
double max_shift_distance;
do {
max_shift_distance = 0;
for(int i=0; i<shifted_points.size(); i++){
if (!stop_moving[i]) {
Vector3d point_new = shift_point(shifted_points[i], points, kernel_bandwidth);
double shift_distance = euclidean_distance(point_new, shifted_points[i]);
if(shift_distance > max_shift_distance){
max_shift_distance = shift_distance;
}
if(shift_distance <= EPSILON) {
stop_moving[i] = true;
}
shifted_points[i] = point_new;
}
}
//printf("max_shift_distance: %f\n", max_shift_distance);
} while (max_shift_distance > EPSILON);
return shifted_points;
}
task main()
{
pid_controller_t left_pid_controller;
init_drive_pid_controller(&left_pid_controller);
pid_controller_t right_pid_controller;
init_drive_pid_controller(&right_pid_controller);
reset_readings();
while (1)
{
check_and_switch_mode();
switch (robot_mode)
{
case Do_Nothing:
stop_moving();
motor[sonar_rotate] = 0;
StopTask(sonar_scanner);
break;
case Turn:
turn_left();
break;
case Follow_Closest_Object:
// keep moving the sonar
StartTask(sonar_scanner);
// turn the robot towards the closest object
int closest_bin = find_closest_sonar_bin();
writeDebugStreamLine("closest_bin: %d", closest_bin);
int degrees_to_turn = closest_bin * (360/SONAR_BINS);
//writeDebugStreamLine("degrees_to_turn: %d", degrees_to_turn);
while (degrees_to_turn > 180)
{
degrees_to_turn -= 360;
}
while (degrees_to_turn < -180)
{
degrees_to_turn += 360;
}
//writeDebugStreamLine("degrees_to_turn: %d", degrees_to_turn);
if (degrees_to_turn < 360/SONAR_BINS)
{
move_motor_to_position(right,
nMotorEncoder[right] + RIGHT_MOTOR_ENCODING_TICKS_PER_DEGREE * degrees_to_turn,
&right_pid_controller);
//turn_left();
}
else if (degrees_to_turn > 360/SONAR_BINS)
{
move_motor_to_position(left,
nMotorEncoder[left] + LEFT_MOTOR_ENCODING_TICKS_PER_DEGREE * degrees_to_turn,
&left_pid_controller);
//turn_right();
}
else
{
go_straight();
}
wait1Msec(1);
break;
default:
return;
}
}
}
