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;
}
// usetup is called during the calibration period. It must return before the
// period ends.
int usetup (void) {
printf("\nPlace robot, press go.");
go_click ();
printf ("\nStabilizing...");
pause (500);
printf ("\nCalibrating offset...\n");
gyro_init (GYRO_PORT, LSB_US_PER_DEG, 500L);
return 0;
}
int usetup (void) {
printf("\nPlace robot, press go.");
go_click ();
printf ("\nStabilizing...");
pause (500);
printf ("\nCalibrating offset...\n");
gyro_init (GYRO_PORT, LSB_US_PER_DEG, 500L);
extern volatile uint8_t robot_id;
robot_id = 128;
extern volatile uint8_t light_port;
light_port = 4;
return 0;
}
void setup_state() {
while (state == SETUP) {
create_thread(sing, 64, 127, "sing_thread");
printf("\nPress go");
go_click();
printf("\nStabilizing");
pause(1000);
printf("\nInitializing");
gyro_init(8,1357.348162*3838.0/3600.0*1028.0/1080.0,5000);
target_angle = 0;
reset_pid_controller(target_angle);
setup_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() {
uint16_t i,n = 36;
uint16_t port=23;
uint16_t km,kc;
uint16_t xd[36];
uint16_t yd[36];
//happylib_init();
// start
printf("\nIRDistCal Press Go");
go_click();
// get number of samples to read
while (!go_press()) {
printf("\nUse frob to # ofsamples: %2d",n);
switch (frob_read_range(0,2)) {
case 0: n= 9; break;
case 1: n=18; break;
case 2: n=36; break;
}
pause (40);
}
// wait for go release
while (go_press());
// fill distance array
for (i=0;i<n;i++) {
xd[i] = 10 + 2*i*(36/n);
}
// get port number
while (!go_press()) {
port = frob_read_range(8,23);
printf("\nUse frob to select port: %2d",port);
pause (40);
}
// wait for go release
while (go_press());
// read samples
for (i=0;i<n;i++) {
while (!go_press()) {
yd[i] = analog_read(port);
printf("\nSample @ %2dcm =%4d",xd[i],yd[i]);
}
// wait for go release
while (go_press());
}
// calculate & print
irdist_fit(xd,yd,n,&km,&kc);
printf("\nOK: M: %5d C: %d, press Go",km,kc);
go_click();
// save
/*
Disabled until confdb is working
printf("\nGo to Save calibStop to quit");
while (1) {
if (go_press()) {
go_click();
confdb_save_integer(CONF_HLIB_IRDIST_M,km);
confdb_save_integer(CONF_HLIB_IRDIST_C,kc);
break;
}
if (stop_press()) {
stop_press();
break;
}
}
*/
printf("\ncalibration done");
// do nothing forever
while (1);
return 0;
}
