Beispiel #1
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;
}
Beispiel #2
0
int umain (void) {
	encoder_reset(RIGHT_ENCODER);
	encoder_reset(LEFT_ENCODER);
	
	while(1)
	{
	motor_set_vel(LEFT_MOTOR, TEST_SPEED);
	motor_set_vel(RIGHT_MOTOR, 0);
	
	
	printf("\nRight: %d, Left: %d, %c", encoder_read(RIGHT_ENCODER), encoder_read(LEFT_ENCODER), 3);
	
	if (stop_press()) {
		motor_set_vel(LEFT_MOTOR, 0);
		motor_set_vel(RIGHT_MOTOR, 0);
		encoder_reset(RIGHT_ENCODER);
		encoder_reset(LEFT_ENCODER);
		printf("\nRight: %d, Left: %d, %c", encoder_read(RIGHT_ENCODER), encoder_read(LEFT_ENCODER), 3);
		go_click();
		// Poliwhirl~
	}
	
	pause(100);
	}
	return 0;
}
Beispiel #3
0
// range [-1,1]
// values out of bounds will be clipped to rangeht
void set_wheel_pows(float l, float r) {
  // printf("set wheel pows to %f and %f\n", l, r);
  motor_set_vel(PIN_MOTOR_DRIVE_L, l >= -1 ? (l <= 1 ? (l * 255) : 255) : -255);
  motor_set_vel(PIN_MOTOR_DRIVE_R, r >= -1 ? (r <= 1 ? (r * 255) : 255) : -255);

  // flush_hybrid_position_integration();
  // flush_encoder_position_integration();

  // cache vals
  last_motor_pows.l = l;
  last_motor_pows.r = r;
}
int umain(void) {
    printf ("\nwait 3 sec...");
    motor_set_vel (0, 192);
    pause(3000);
    panic ("stop");
    return 0;
}
Beispiel #5
0
void moving_state() {
	printf("\nMoving state");
	left_encoder_base = encoder_read(LEFT_ENCODER);
	right_encoder_base = encoder_read(RIGHT_ENCODER);
	while(state == MOVING)
	{
		float input = gyro_get_degrees();
		
		float output = update_pid_input(&controller, input);
		
		motor_set_vel(RIGHT_MOTOR, FORWARD_SPEED + (int)output + OFFSET_ESTIMATE);
		motor_set_vel(LEFT_MOTOR, FORWARD_SPEED - (int)output - OFFSET_ESTIMATE);
		
		pause(50);
		
		moving_filter();
	}
}
Beispiel #6
0
void term_process(void) {
	//TODO exec commands
	enum CMD cmd_i = 255;
	for(unsigned char i=0; i<ARRAY_SIZE(cmds); i++) {
		if(cmp(i, buff)) cmd_i = i;
	}
	
	unsigned int a, b;
	float fa, fb;
	switch(cmd_i) {
		case HELP:
			bprintf("< commands >\n");
			bprintf("set\t- edit int value\n"
					"fset\t- edit float value\n"
					"all\t- view all\n"
					"view\t- view one\n"
					"follow\t- view changes\n"
					"motor\t- control motor\n");
			break;
		case SET:
			sscanf(buff, "%*s %d %d", &a, &b);
			bprintf("term: id=%d val=%d", a, b);
			dbg_set(a, &b, INT);
			break;
		case FSET:
			sscanf(buff, "%*s %d %d", &a, &b);
			fa = (float)a;
			dbg_set(a, &fa, FLOAT);
			break;
		case ALL:
			bprintf("| %d vars\t|\n", dbg_watch_count);
			for(unsigned char i=0; i<dbg_watch_count; i++) {
				bprintf("%d:\t", i);
				dbg_print(i);
				bprintf("\n");
			}
			break;
		case VIEW:
			sscanf(buff, "%*s %d", &a);
			bprintf("val is ");
			dbg_print(a);
			break;
		case FOLLOW:
			sscanf(buff, "%*s %d", &dbg_index);
			mode = FOLLOWING;
			create_thread(&update, STACK_DEFAULT, 1, "follower_thread");
			break;
		case MOTOR:
			sscanf(buff, "%*s %d %d", &a, &b);
			motor_set_vel(a, b);
			break;
		default:
			bprintf("not understood.\n");
	}
}
Beispiel #7
0
void turning_state() {
	printf("\nTurning state");
	while(state == TURNING) {
		
		float angle = gyro_get_degrees();
		
		//printf("\n%f  %f", angle, target_angle);
		
		if (target_angle > angle) {
			motor_set_vel(RIGHT_MOTOR, clamp((target_angle - angle)/2.0 + 40, -TURNING_SPEED, TURNING_SPEED));
			motor_set_vel(LEFT_MOTOR, clamp((angle - target_angle)/2.0 - 40, -TURNING_SPEED, TURNING_SPEED));
		} else {
			motor_set_vel(RIGHT_MOTOR, clamp((target_angle - angle)/2.0 - 40, -TURNING_SPEED, TURNING_SPEED));
			motor_set_vel(LEFT_MOTOR, clamp((angle - target_angle)/2.0 + 40, -TURNING_SPEED, TURNING_SPEED));
		}
		pause(50);
		
		turning_filter();
	}
}
void test_motors() {
    uint8_t mot;
    uint16_t pos;
    while(1){
        printf("Please choose a motor to test: (0 or 1 please)\n");
        scanf("%d", &mot);
        if (mot >= 0 && mot <= 6){
            printf("Push Go to start testing Motor %d. Push stop at any time to restart the test.\nNote that 0 on the knob corresponds to moving backwards at -255, 255 on knob corresponds to stopped, and 511 on knob corresponds to moving forwards at 255.\n\n", mot);
            break;
        }
        else
            printf("Not a valid motor. Please try again.\n\n");
    }
    go_click();
    printf("Now Testing Motor %d", mot);
    while (!stop_press()) {
        pos = frob_read()/2;
        printf("Motor %d is set to %3d with a current of %dmA\n",mot,pos,motor_get_current_MA(mot));
        motor_set_vel(mot,pos-256);
        pause(50);
    }
    motor_set_vel(mot,0);
}
Beispiel #9
0
int umain (void) {
    while (1)
    {
    	motor_set_vel(0, 100);
		motor_set_vel(1, 100);
		pause(3000);
		motor_set_vel(0, 0);
		motor_set_vel(1, 0);
		pause(3000);
		motor_set_vel(0, 70);
		motor_set_vel(1, -70); //find out what direction this turns it
		pause(3000);

    }
    return 0;
}
Beispiel #10
0
void SetRightVelocity(float vel)
{
    motor_set_vel(RIGHT_MOTOR, (int16_t)FixDeadZone(vel)); }
Beispiel #11
0
void MotorCoast(uint8_t Motor)
{
    motor_set_vel(Motor, 0);
}
Beispiel #12
0
void dec_left(){
    if (left_mot_vel > 70){
        left_mot_vel--;
        motor_set_vel(LEFT_MOTOR,left_mot_vel);
    }
}
Beispiel #13
0
void inc_left(){
    if (left_mot_vel < 80){
        left_mot_vel++;
        motor_set_vel(LEFT_MOTOR,left_mot_vel);
    }
}
Beispiel #14
0
void setMotorsVelocity(float leftV, float rightV)
{

	motor_set_vel(0, leftV);
	motor_set_vel(1, rightV);
}
Beispiel #15
0
void SetLeftVelocity(float vel)
{
    motor_set_vel(LEFT_MOTOR, (int16_t)FixDeadZone(vel)); }
Beispiel #16
0
void turnAndDrive(float MV)
{
    int angle = (int)runGetError();
    //printf("\n%i",angle);
    if (!isForward) {
        //printf("\n->%i",angle);
        if (angle < -RUN_CUTOFF_ANGLE) {
            //printf("\nTURNRIGHT");
            motor_set_vel(RIGHT_MOTOR,-TURN_VEL);
            motor_set_vel(LEFT_MOTOR,TURN_VEL);
        } else if (angle > RUN_CUTOFF_ANGLE) {
            //printf("\nTURNLEFT");
            motor_set_vel(RIGHT_MOTOR,TURN_VEL);
            motor_set_vel(LEFT_MOTOR,-TURN_VEL);
        } else {
            //printf("\nSTRAIGHT, %.2f", MV);
            motor_set_vel(RIGHT_MOTOR,CLAMP(targetVel-(int)MV));
            motor_set_vel(LEFT_MOTOR,CLAMP(targetVel+(int)MV));
        }
    } else {
        //printf("\nOTHER");
        if (angle < -RUN_CUTOFF_ANGLE) {
            motor_set_vel(RIGHT_MOTOR,-TURN_VEL);
            motor_set_vel(LEFT_MOTOR,TURN_VEL);
        } else if (angle > RUN_CUTOFF_ANGLE) {
            motor_set_vel(RIGHT_MOTOR,TURN_VEL);
            motor_set_vel(LEFT_MOTOR,-TURN_VEL);
        } else {
            motor_set_vel(RIGHT_MOTOR,REV_CLAMP(-targetVel-(int)MV));
            motor_set_vel(LEFT_MOTOR,REV_CLAMP(-targetVel+(int)MV));
        }
    }
}
Beispiel #17
0
void soft_stop_motors(int duration) {
	motor_set_vel(RIGHT_MOTOR, 0);
	motor_set_vel(LEFT_MOTOR, 0);
	pause(duration);
}