Calculo de la Trayectoria 3º

Calculo de la Trayectoria
Parte 3º

En este apartado nos pedían que como conseguir mediante dos arcos que el robot llegara a su destino con la orientación final.
Para este ejercicio lo que hemos pensado que seria mejor, es realizar un punto intermedio del destino a donde quieres llegar, y dirigirse primero hacia ese destino.
Una vez en el primer destino, tendríamos que realizar otro arco en la dirección opuesta para poder quedarnos en la posición indicada.
Es importante indicar que la orientación en la que tendría que empezar el robot depende de la orientación destino que nos indique, ya que tendríamos que orientarnos de forma similar a la orientación destino para finalmente, cuando realicemos el segundo arco terminar en una posición similar.
También es de interés la amplitud del primer arco, ya que si la orientacion tiene unos valores altos, el primer arco tendría que alejarse bastante del punto para poder realizar en el segundo arco un giro de mayor amplitud.
Aquí indicamos con pseudocódigo como quedaría:

Tomamos los valores de la X,Y,Ang
Calculamos la distancia hacia el punto
Dividimos la distancia en dos puntos dependiendo del ángulo de destino.
Calculamos el primer arco, con un radio determinado y un ángulo de giro dependiendo del ángulo final
Realizamos el segundo arco.