Fuzzy logic Autonomous Car

Hosting TCL code for the vedio , http://www.youtube.com/watch?v=4r9Ou-JC4ao

package require Tk
proc get_slope {value} {
global INPUT_DOMAIN
global OUTPUT_DOMAIN
if {$value <= [lindex $INPUT_DOMAIN 0] } {
# puts "enering this loop1 [lindex $INPUT_DOMAIN 0] <= $value"
set prob 1
set angle [expr $prob * [lindex $OUTPUT_DOMAIN 0]]
} elseif {$value > [lindex $INPUT_DOMAIN 0] && $value < [lindex $INPUT_DOMAIN 1] } {
# puts "enering this loop2 [expr abs([expr [lindex $INPUT_DOMAIN 1] - $value])]"
set int_with_0 [expr [expr abs([expr [lindex $INPUT_DOMAIN 1] - $value])] / 10.0]
set int_with_1 [expr [expr abs([expr [lindex $INPUT_DOMAIN 0] - $value])] / 10.0]
set angle_0 [expr $int_with_0 * [lindex $OUTPUT_DOMAIN 0]]
set angle_1 [expr $int_with_1 * [lindex $OUTPUT_DOMAIN 1]]
set angle [expr $angle_0 + $angle_1]
} elseif {$value == [lindex $INPUT_DOMAIN 1]} {
# puts "enering this loop3"
set prob 1
set angle [expr $prob * [lindex $OUTPUT_DOMAIN 1]]
} elseif {$value > [lindex $INPUT_DOMAIN 1] && $value < [lindex $INPUT_DOMAIN 2] } {
# puts "enering this loop4"
set int_with_1 [expr [expr abs([expr [lindex $INPUT_DOMAIN 2] - $value])] / 10.0]
set int_with_2 [expr [expr abs([expr [lindex $INPUT_DOMAIN 1] - $value])] / 10.0]
set angle_0 [expr $int_with_1 * [lindex $OUTPUT_DOMAIN 1]]
set angle_1 [expr $int_with_2 * [lindex $OUTPUT_DOMAIN 2]]
set angle [expr $angle_0 + $angle_1]
} elseif {$value == [lindex $INPUT_DOMAIN 2]} {
# puts "enering this loop5"
set prob 1
set angle [expr $prob * [lindex $OUTPUT_DOMAIN 2]]
} elseif {$value > [lindex $INPUT_DOMAIN 2] && $value < [lindex $INPUT_DOMAIN 3] } {
# puts "enering this loop6"
set int_with_2 [expr [expr abs([expr [lindex $INPUT_DOMAIN 3] - $value])] / 10.0]
set int_with_3 [expr [expr abs([expr [lindex $INPUT_DOMAIN 2] - $value])] / 10.0]
set angle_0 [expr $int_with_2 * [lindex $OUTPUT_DOMAIN 2]]
set angle_1 [expr $int_with_3 * [lindex $OUTPUT_DOMAIN 3]]
set angle [expr $angle_0 + $angle_1]
} elseif {$value == [lindex $INPUT_DOMAIN 3]} {
# puts "enering this loop7"
set prob 1
set angle [expr $prob * [lindex $OUTPUT_DOMAIN 3]]
} elseif {$value > [lindex $INPUT_DOMAIN 3] && $value < [lindex $INPUT_DOMAIN 4] } {
# puts "enering this loop8"
set int_with_3 [expr [expr abs([expr [lindex $INPUT_DOMAIN 4] - $value])] / 10.0]
set int_with_4 [expr [expr abs([expr [lindex $INPUT_DOMAIN 3] - $value])] / 10.0]
set angle_0 [expr $int_with_3 * [lindex $OUTPUT_DOMAIN 3]]
set angle_1 [expr $int_with_4 * [lindex $OUTPUT_DOMAIN 4]]
set angle [expr $angle_0 + $angle_1]
} elseif {$value >= [lindex $INPUT_DOMAIN 4]} {
# puts "enering this loop9"
set prob 1
set angle [expr $prob * [lindex $OUTPUT_DOMAIN 4]]
}
return $angle
#puts "Angle for $value is $angle"
}
proc xvalue { loc angle} {
set x [lindex $loc 0]
set y [lindex $loc 1]
return [expr ($x * cos(($angle*3.14159265)/180)) - ($y * sin(($angle*3.14159265)/180)) ]
}
proc yvalue { loc angle } {
set x [lindex $loc 0]
set y [lindex $loc 1]
return [expr ($y * cos(($angle*3.14159265)/180)) + ($x * sin(($angle*3.14159265)/180)) ]
}

proc update_car { can loc angle } {
global LEFT_SENSOR
global RIGHT_SENSOR
catch { $can delete car }
#catch { $can delete line }

set newx [xvalue $loc -$angle]
set newy [yvalue $loc -$angle]

set temp_loc "[expr $newx - 30] [expr $newy - 15]"

set line_corrds "[xvalue $temp_loc  $angle] [yvalue $temp_loc  $angle] "

set temp_loc "[expr $newx + 30] [expr $newy - 15]"
set LEFT_SENSOR "[xvalue $temp_loc  $angle] [yvalue $temp_loc  $angle]"
eval "$can create rect  [lindex $LEFT_SENSOR 0] [lindex $LEFT_SENSOR 1] [expr [lindex $LEFT_SENSOR 0] + 10] [expr [lindex $LEFT_SENSOR 1] + 10] -fill blue -tag car"
set line_corrds "$line_corrds  $LEFT_SENSOR  "
puts "right sensor $LEFT_SENSOR"

set temp_loc "[expr $newx + 30] [expr $newy + 15]"
set RIGHT_SENSOR "[xvalue $temp_loc  $angle] [yvalue $temp_loc  $angle]"
eval "$can create rect  [lindex $RIGHT_SENSOR 0] [lindex $RIGHT_SENSOR 1] [expr [lindex $RIGHT_SENSOR 0] + 10] [expr [lindex $RIGHT_SENSOR 1] + 10] -fill yellow -tag car"

set line_corrds "$line_corrds  $RIGHT_SENSOR "

set temp_loc "[expr $newx - 30] [expr $newy + 15]"
set line_corrds "$line_corrds  [xvalue $temp_loc  $angle] [yvalue $temp_loc  $angle] "

eval "$can create polygon $line_corrds -fill green -tag car"
#eval "$can create line

}
#set save_file [open road.txt r]
#close $save_file
proc addLine {x y} {
     global side_lines ;
    set save_file [open road.txt a]
    puts $save_file ".canvas create line [.canvas canvasx $::lastx] [.canvas canvasy  $::lasty] [.canvas canvasx $x] [.canvas canvasy $y]"
    puts $save_file "set side_lines \"\$side_lines {  {[.canvas canvasx $::lastx] [.canvas canvasy  $::lasty]} {[.canvas canvasx $x] [.canvas canvasy $y]} } \" "
    close $save_file
    .canvas create line [.canvas canvasx $::lastx] [.canvas canvasy  $::lasty] [.canvas canvasx $x] [.canvas canvasy $y]

    set side_lines "$side_lines {  {[.canvas canvasx $::lastx] [.canvas canvasy  $::lasty]} {[.canvas canvasx $x] [.canvas canvasy $y]} } "

    set ::lastx $x; set ::lasty $y
}

#############################################################################

 proc Intersect {p1 p2 p3 p4} {
    return [IntersectV $p1 [VSub $p2 $p1] $p3 [VSub $p4 $p3]]
 }
 proc IntersectV {p1 v1 p3 v3} {
    foreach {x1 y1} $p1 {vx1 vy1} $v1 {x3 y3} $p3 {vx3 vy3} $v3 break

    set a $vx1
    set b [expr {-1 * $vx3}]
    set c $vy1
    set d [expr {-1 * $vy3}]
    set e [expr {$x3 - $x1}]
    set f [expr {$y3 - $y1}]

    set det [expr {double($a*$d - $b*$c)}]
    if {$det == 0} {return 0}

    set k [expr {($d*$e - $b*$f) / $det}]
    #set j [expr {($a*$f - $c*$e) / $det}]
    return [VAdd $p1 $v1 $k]
 }


 proc VAdd {v1 v2 {scaling 1}} {
    foreach {x1 y1} $v1 {x2 y2} $v2 break
    return [list [expr {$x1 + $scaling*$x2}] [expr {$y1 + $scaling*$y2}]]
 }
 proc VSub {v1 v2} { return [VAdd $v1 $v2 -1] }
 proc VCross {v1 v2} {
    foreach {x1 y1} $v1 {x2 y2} $v2 break
    return [expr {($x1*$y2) - ($y1*$x2)}]
 }
 proc VRotate {v beta} {
    foreach {x y} $v break
    set xx [expr {$x * cos(-$beta) - $y * sin(-$beta)}]
    set yy [expr {$x * sin(-$beta) + $y * cos(-$beta)}]
    return [list $xx $yy]
 }

proc sensor_reading {POS DIR DEPTH tag} {
global side_lines ;
set POS_EXT "[expr [lindex $POS 0] + ($DEPTH *cos($DIR * (3.14159265 /180) ))] [expr [lindex $POS 1] + ($DEPTH *sin($DIR * (3.14159265 /180) ))] "
set current_reading 100000
# puts ".canvas create line [lindex $POS 0] [lindex $POS 1] [lindex $POS_EXT 0] [lindex $POS_EXT 1] -tag sensor_lines"
.canvas create line [lindex $POS 0] [lindex $POS 1] [lindex $POS_EXT 0] [lindex $POS_EXT 1] -tag sensor_lines
foreach line_seg $side_lines  {
foreach {C1 C2} $line_seg { break }
set result [Intersect $POS $POS_EXT $C1 $C2 ]
if { $result == 0 } { continue }
set xmax -100
set ymax -100
set xmin 100000
set ymin 100000
foreach xin  "{$POS} {$POS_EXT} {$C1} {$C2}" {
set x [lindex $xin 0]
set y [lindex $xin 1]
if { $xmax < $x } { set xmax $x }
if { $ymax < $y } { set ymax $y }
if { $xmin > $x } { set xmin $x }
if { $ymin > $y } { set ymin $y }
}
set smallest_right_hand_vale  [lindex $POS_EXT 0]
if { [lindex $POS_EXT 0] > [lindex $C2 0] } {
set smallest_right_hand_vale  [lindex $C2 0]
}
set largest_left_hand_vale  [lindex $POS 0]
if { [lindex $POS 0] < [lindex $C1 0] } {
set largest_left_hand_vale  [lindex $C1 0]
}

set smallest_right_hand_valey  [lindex $POS 1]
if { [lindex $POS 1] > [lindex $C1 1] } {
set smallest_right_hand_valey  [lindex $C1 1]
}
set largest_left_hand_valey  [lindex $POS_EXT 1]
if { [lindex $POS_EXT 1] < [lindex $C2 1] } {
set largest_left_hand_valey  [lindex $C2 1]
}

if { [lindex $C1 0] > [lindex $C2 0] } {
set t_max  [lindex $C1 0]
set t_min  [lindex $C2 0]
} else {
set t_max  [lindex $C2 0]
set t_min  [lindex $C1 0]
}

if { [lindex $C1 1] > [lindex $C2 1] } {
set ty_max  [lindex $C1 1]
set ty_min  [lindex $C2 1]
} else {
set ty_max  [lindex $C2 1]
set ty_min  [lindex $C1 1]
}
####################
if { [lindex $POS 0] > [lindex $POS_EXT 0] } {
set t_max1  [lindex $POS 0]
set t_min1  [lindex $POS_EXT 0]
} else {
set t_max1  [lindex $POS_EXT 0]
set t_min1  [lindex $POS 0]
}

if { [lindex $POS 1] > [lindex $POS_EXT 1] } {
set ty_max1  [lindex $POS 1]
set ty_min1  [lindex $POS_EXT 1]
} else {
set ty_max1  [lindex $POS_EXT 1]
set ty_min1  [lindex $POS 1]
}

#if { [lindex $result 0] <= $smallest_right_hand_vale && [lindex $result 0] >= $largest_left_hand_vale &&
#    [lindex $result 1] <= $largest_left_hand_valey && [lindex $result 1] >= $smallest_right_hand_valey &&
# }
if {  $t_min <= [lindex $result 0] && $t_max >= [lindex $result 0] &&
   $ty_min <= [lindex $result 1] && $ty_max >=  [lindex $result 1] &&
    $t_min1 <= [lindex $result 0] && $t_max1 >= [lindex $result 0] &&
   $ty_min1 <= [lindex $result 1] && $ty_max1 >=  [lindex $result 1]
      } {
set temp_x [expr sqrt((([lindex $POS 0] - [lindex $result 0])*([lindex $POS 0] - [lindex $result 0]))+(([lindex $POS 1] - [lindex $result 1])*([lindex $POS 1] - [lindex $result 1])))]
if { $temp_x < $current_reading } {
catch { .canvas delete $tag ;}
eval "catch {.canvas delete sensor_lines_my_$tag}"
eval ".canvas create oval [expr [lindex $result 0] - 2] [expr [lindex $result 1] - 2] [expr [lindex $result 0] + 2] [expr [lindex $result 1] + 2] -fill yellow -tag $tag"
eval ".canvas create line [lindex $C1 0] [lindex $C1 1] [lindex $C2 0] [lindex $C2 1]  -fill red -width 10 -tag sensor_lines_my_$tag"

# puts "Changing value to $temp_x from $current_reading because $POS && $result ->  $POS $POS_EXT $C1 $C2 "
set current_reading $temp_x
} else {
# puts "Current reading is less than the present valyue $temp_x $current_reading"
}
} else {
# puts "Intersection is invalid $result --xmax $xmax- xmin -$xmin- ymax -$ymax- ymin -$ymin --- "
}
}
puts "Sensor Reading $current_reading"
return $current_reading
}

## FUZZY Logic
proc gaussian_val { x U sig } {
return [expr exp(-(($x-$U)*($x-$U))/(2*($sig*$sig)))]
}
catch { destroy .canvas }
grid [canvas .canvas] -sticky nwes -column 0 -row 0
grid columnconfigure . 0 -weight 1
grid rowconfigure . 0 -weight 1

bind .canvas <1> "set lastx %x; set lasty %y"
bind .canvas "addLine %x %y"

set side_lines ""

set current_loc "100 100"
set current_angle "45"


set INPUT_DOMAIN "-400 -200 0 200 400"
set SIG_DOMAIN_MAP "2.726 2.726 2.726 2.726 2.726"
set OUTPUT_DOMAIN "20 10  0 -10 -20"

set DEPTH 2000

set go_straight 0
proc start_car {} {
global go_straight
global side_lines ;
global RIGHT_SENSOR
global LEFT_SENSOR

global current_loc
global current_angle
global INPUT_DOMAIN
global SIG_DOMAIN_MAP
global OUTPUT_DOMAIN
global DEPTH

set test_cnt 0

while { 1 } {
incr test_cnt
if { $test_cnt > 100 } { break ;}
update_car .canvas $current_loc  $current_angle

catch {.canvas delete sensor_lines}
catch {.cavnas delete cir_right}
set sensor1 [sensor_reading $RIGHT_SENSOR $current_angle $DEPTH "cir_right"] ; #Left

catch {.cavnas delete cir_left}
set sensor2 [sensor_reading $LEFT_SENSOR $current_angle  $DEPTH "cir_left"] ; #Right

puts "Right Sensor reading $sensor1 LEFT reading $sensor2"
if { $sensor1 == 100000 || $sensor2 == 100000 } { puts "ERROR : got Zero reading for sensor : ending COde " ;
incr go_straight 1
set Theta 0
if { $go_straight > 100 } {
break ;
}
} else {
set go_straight 0
}

if { $go_straight == 0 } {
set diff [expr $sensor2 - $sensor1 ]
set least $sensor1
if { $sensor2 < $least } {
set least $sensor2
}
puts "Got Diff $diff $least "
set diff [expr ($diff / exp(($least-80)/5))  ]
if { [expr abs($diff)] < 0.07 && $least < 20} {
set diff [expr $diff * 600 ]
} elseif { [expr abs($diff)] < 0.1 && $least < 50} {
set diff [expr $diff * 100 ]
} elseif { [expr abs($diff)] < 1 } {
# set diff [expr $diff * 10 ]
} elseif { [expr abs($diff)] < 5 } {
# set diff [expr $diff * 5 ]
}
puts "Got Diff after mult $diff"
# check for upper & lower bounds
if { $diff < [lindex $INPUT_DOMAIN 0] } {
set Theta [lindex $OUTPUT_DOMAIN 0]
} elseif  {$diff > [lindex $INPUT_DOMAIN end]} {
set Theta [lindex $OUTPUT_DOMAIN end]
} else {
set cnt -1
set Theta [get_slope $diff]
#foreach x $INPUT_DOMAIN {
# incr cnt
#
# #set Theta [expr  $Theta + ([lindex $OUTPUT_DOMAIN $cnt ] * get_slope [gaussian_val $diff $x [lindex $SIG_DOMAIN_MAP $cnt] ] ) ]
# set Theta [expr  $Theta + [get_slope $diff] ]
# puts "Current $Theta in $cnt"
#}

}

set current_angle [expr $current_angle  + $Theta ]
}
set car_movement_limit 10
puts "Hi $current_loc $Theta $current_angle "
set current_loc "[expr [lindex $current_loc  0] + ($car_movement_limit *cos($current_angle * (3.14159265 /180) ))] [expr [lindex $current_loc 1] + ($car_movement_limit *sin($current_angle * (3.14159265 /180) ))] "

puts "$current_angle $current_loc "
after 300 start_car
break

}  ; # end of frame

}  ; ## end of start car
if { [file exists road.txt ] } {
source road.txt
start_car
}