void Robot::spin() {//S型过人
cv::Point2f robot_coord(x, y);
std::vector<cv::Point2f> ans = findMulBall();
cv::Point2f ball1 = ans[0];
cv::Point2f ball2 = ans[1];
//printf("%f %f\n %f %f\n", ball1.x, ball1.y, ball2.x, ball2.y);
float delta = 30;
float r12 = cal_distance(ball1, ball2) / 2;
float rspin = (delta * delta + r12 * r12) / (2 * delta);
rspin += 10;//误差修正
float disr1 = cal_distance(ball1, robot_coord);
int inrspin = 0;
while(inrspin < rspin)
inrspin++;
//printf("rspin:%f, disr1:%f, inrspin:%d\n", rspin, disr1, inrspin);
cv::Point2f rto1 = ball1 - robot_coord;
cv::Point2f b1to2 = ball2 - ball1;
float arc = asin(r12 / rspin);
cv::Point2f pturn1(ball1.x - b1to2.x * 0.5, ball1.y - b1to2.y *0.5);
moveTo(pturn1, 20);
cv::Point2f robotPosition(x,y);
cv::Point2f dir= ball1 - robotPosition;
float dir_len = length(dir);
if (dir_len <= 0)
return;
cv::Point2f new_dir = dir*(1/dir_len);
rotateTo(new_dir);
turnRight(arc * 180 / M_PI);
moveRotate(true, rspin, arc * 2 + 0.2);//误差修正
moveRotate(false, rspin, arc * 2 + 0.2);
}
int main()
{
float duration = 0.0;
float distance = 0.0;
status = false;
while(1)
{
triggerpin = 0;
wait_us(1); // Wait for 1us for clean low pulse
triggerpin = 1;
wait_us(10); // Trigger Pulse of 10us
triggerpin = 0;
while(!echopin); //Poll echo pin for High
timer.start(); //start Timer
while(echopin); //Poll echo pin for low
timer.stop(); //Stop timer
duration = (float)(timer.read_us()); //Read Timer Value = Echo High Time
//terminal.printf("\n Timer Reading cm : %f \n", duration);
distance = cal_distance(duration);
terminal.printf("\n Object Distance cm : %f \n", distance);
timer.reset(); //Reset Timer
wait(1); // Repeat Trigger after 1sec
}
}
__interrupt void TIMER1_A1_ISR_HOOK(void)
{
/* USER CODE START (section: TIMER1_A1_ISR_HOOK) */
/* replace this comment with your code */
switch(__even_in_range(TA1IV, 10))
{
case TA1IV_TACCR1:{
ccr1_count = TA1CCR1;
///////////////////////////////////////
distance[cycle_count-1]=cal_distance();
// uart_send_num(cal_distance());
// uart_send_string("\r\n");
TA1CCR1=0;
TA1R=0;
TA1CTL &=~ MC_2;
TA1CCTL1 &=~ CCIFG;
break;
}
case TA1IV_TAIFG:{
ccr1_count += 65535;
TA1CCR1=0;
TA1R=0;
break;
}
default:break;
}
/* USER CODE END (section: TIMER1_A1_ISR_HOOK) */
}
