math_vector math_vector::rotate(double degrees){
// printf("%g degrees + %g degrees\n",getAngle(),degrees);
if (!degrees) return math_vector(x,y);
if (degrees==270) return rightAngle();
if (degrees==180) return (*this)*-1;
if (degrees==90) return rightAngle()*-1;
double angle=getAngle()+degrees;
while (angle<0) angle+=360;
while (angle>360) angle-=360;
return createFromPolar(angle,getMagnitude());
}
int main()
{
printf("beginning camera calibration pgm\n");
create_connect();
enable_servos();
camera_open(LOW_RES);
printf("1.0 after initialization\n");
//place create for camera recognition of cube positions
forward(9);
rightAngle(LEFT);
forward(15);
rightAngle(RIGHT);
calibrateCamera();
printf("the end...\n");
camera_close();
create_disconnect();
return 0;
}
void main(void)
{
vInitialize();
myInit();
while(!pushsw_get())
{
if(cnt1<100)
led_out(0x1);
else if(cnt1<200)
led_out(0x2);
else cnt1 = 0;
}
cnt1 = 0;
while(startbar_get())
{
if(cnt1<50)
led_out(0x1);
else if(cnt1<100)
led_out(0x2);
else cnt1 = 0;
}
led_out(0);
start();
pattern = 10;
while(1)
{
switch(pattern)
{
case 10: //直线
if(check_crossline()){ pattern = 30;break; }
if(check_blackArea()){ pattern = 40;break; }
straight_run(); //直线前行，弯道检测，盲区检测
break;
case 20: //弯道
switch(turn_direction)
{
case LEFT:
handle(-1*midDegree_st4);
left_turn();
break;
case RIGHT:
handle(midDegree_st4);
right_turn();
break;
default:break;
}
break;
case 30: //直角
rightAngle();
break;
case 40: //盲区
switch(blackArea_direction)
{
case LEFT:
left_blackArea();
break;
case RIGHT:
right_blackArea();
break;
default:break;
}
break;
default:break;
}//end switch
}//end while
}
