KentLibby_hwk2.jl

#=
Kent Libby Homework 2, 11/03/18
=#
clearconsole()
using Plots; gr()
#=
Excercise 1.1:
1. The formula needed is V=omega(rad/s)*Diameter/2
=#

# 2.

targetSpeed= 40 #in cm/s
diameter= 20 #in cm/s
wheelSpeed=targetSpeed/(diameter/2)
println("Excercise 1.1")
println("1. The formula needed is V=omega(rad/s)*Diameter/2")
println("2. The speed in rad/sec needed to move at 40 cm/s is $wheelSpeed rad/s")

#3.
targetTheta=2*pi
interWheelL=30 #in cm
targetTime=50 # in seconds
totalV=(targetTheta*interWheelL)/targetTime
Vl=-totalV/2
Vr=-Vl
omegaRight=Vr/(diameter/2)
omegaLeft=Vl/(diameter/2)
println("3. the target speeds for the left and right wheels are $omegaLeft,
and $omegaRight rad/s\n")

#=
Excercise 1.2:
=#
#1.
xLoc=5
yLoc=3
Vr=10 #cm/sec
Vl= 15 #cm/sec
theta=60 #in degrees
R=interWheelL/2*((Vr+Vl)/(Vr-Vl))
ICC=[xLoc-R*sind(theta); yLoc+R*cosd(theta)]
println("Excercise 1.2")
println("1. global ICC is at [x,y] $ICC")
#2.
localCorrection=[xLoc; yLoc]
localICC=ICC-localCorrection
println("2. local ICC is at [x,y] $localICC")

#3.
robotOmega=(Vr-Vl)/interWheelL
dt=20 #seconds
robotTransform=[cos(robotOmega*dt) -sin(robotOmega*dt) 0;
sin(robotOmega*dt) cos(robotOmega*dt) 0;
0 0 1]
robotPos=[xLoc-ICC[1];
yLoc-ICC[2];
theta*pi/180]
offset=[ICC; robotOmega*dt]
finalPos=robotTransform*robotPos+offset
println("3. Robot final pose = $finalPos")

#=
Excercise 2.1
=#
print("\n")
println("Excercise 2.1")
function robotLoc(vl, vr, t)
    L=30 # in cm
    omega=(vr-vl)/L
    R=L/2*((vr+vl)/(vr-vl))
    x=0
    y=0
    theta=30
    if(vr==vl)
        println("vl can't equal vr")
        return 0
    end

    cur_transform=[cos(omega*t) -sin(omega*t) 0;
    sin(omega*t) cos(omega*t) 0;
    0 0 1]

    ICC=[x-R*sind(theta); y+R*cosd(theta)]
    ICC_init=[x-ICC[1]; y-ICC[2]; theta]
    ICC_adjust=[ICC; omega*t]
    final_pos=cur_transform * ICC_init + ICC_adjust
    x=final_pos[1]
    y=final_pos[2]
    return x,y

end

R=10 #in meteres
vl_slow=50.0*.9; vl_fast=50.0*1.1
vr_slow=20.0*.9; vr_fast=20.0*1.1
vr_act=50.0; vl_act=20.0
t = 0 : .01 : 30
ssPoses=robotLoc.(vl_slow, vr_slow, t)
sfPoses=robotLoc.(vl_slow, vr_fast, t)
fsPoses=robotLoc.(vl_fast, vr_slow, t)
ffPoses=robotLoc.(vl_fast, vr_fast, t)
actPoses=robotLoc.(vr_act, vl_act, t)

#this plots the position of the robot across the timespan, the slow slow and fast
# fast configurations have the same result

myPlot=plot([p[1] for p in ssPoses], [p[2] for p in ssPoses], label="slow, slow")
plot!(myPlot,[p[1] for p in sfPoses], [p[2] for p in sfPoses], label="slow, fast")
plot!(myPlot,[p[1] for p in fsPoses], [p[2] for p in fsPoses], label="fast, slow")
plot!(myPlot,[p[1] for p in ffPoses], [p[2] for p in ffPoses], label="fast, fast")
plot!(myPlot,[p[1] for p in actPoses], [p[2] for p in actPoses], label="actual" )
display(myPlot)
print("\n")
#=Excercise 3
=#
println("Excercise 3.1")
interWheelL=30 #in cm
radius=100 # in cm
runTime = 5 #in seconds
diffVRVL=atand(4/7)*interWheelL/runTime
desiredVL=(diffVRVL-radius/7.5*diffVRVL)*7.5/radius
desiredVR=diffVRVL+desiredVL
endTheta=desiredVR-desiredVL/interWheelL*runTime
print("1. the left wheel velocity is $desiredVL and the right wheel is $desiredVR\n")
print("2. the end pose is x=4, y=7, theta=$endTheta\n")

print("\n\n")