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;
}
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;
}
// 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;
}
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();
}
}
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");
}
}
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);
}
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;
}
void SetRightVelocity(float vel)
{
motor_set_vel(RIGHT_MOTOR, (int16_t)FixDeadZone(vel)); }
void MotorCoast(uint8_t Motor)
{
motor_set_vel(Motor, 0);
}
void dec_left(){
if (left_mot_vel > 70){
left_mot_vel--;
motor_set_vel(LEFT_MOTOR,left_mot_vel);
}
}
void inc_left(){
if (left_mot_vel < 80){
left_mot_vel++;
motor_set_vel(LEFT_MOTOR,left_mot_vel);
}
}
void setMotorsVelocity(float leftV, float rightV)
{
motor_set_vel(0, leftV);
motor_set_vel(1, rightV);
}
void SetLeftVelocity(float vel)
{
motor_set_vel(LEFT_MOTOR, (int16_t)FixDeadZone(vel)); }
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));
}
}
}
void soft_stop_motors(int duration) {
motor_set_vel(RIGHT_MOTOR, 0);
motor_set_vel(LEFT_MOTOR, 0);
pause(duration);
}
