argosGoAround/test_leds.lua at master · freedomcondor/argosGoAround · GitHub

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
State = require("State")


statemachine = State:create
{
   id = "root_machine",
   initial = "goStraight",
   substates =
   {
      goStraight = State:create
         {method = function()
			 --print("goStraight");
			 robot.wheels.set_velocity(5,5);return 0;end},
      turnLeft = State:create
         {method = function()
			 --print("turnLeft");
			 robot.wheels.set_velocity(-5,5);return 0; end},
      turnRight = State:create
         {method = function()
			 --print("turnRight");
			 robot.wheels.set_velocity(5,-5);return 0;end},
      goAlongWall = State:create
         {
            id = "goAlongWall",
            initial = "forward",
            substates =
            {
               forward = State:create
               { method = 	function()
				   				--print("subgo");
								robot.wheels.set_velocity(5,5)
                                a = robot.proximity
                                if a[1].value ~= 0 or a[22].value~=0 or
                                   a[2].value ~= 0 or a[23].value~=0 or
                                   a[3].value ~= 0 or a[24].value~=0 then
                                      --return "forwardLeft"
                                      return "EXIT"
                                elseif  a[19].value ~= 0 and
                                    a[20].value ~= 0 and
                                    a[21].value ~= 0 then
                                      return "forwardLeft"
                                elseif a[19].value == 0 and
                                        a[20].value == 0 and
                                        a[21].value == 0 then
                                      return "forwardRight"
                                else
                                      return false
                                end
                           	end
               },
               forwardLeft = State:create
               { method = 	function()
				   				--print("subleft");
								robot.wheels.set_velocity(0,5)
                                a = robot.proximity
                                if  a[19].value ~= 0 and
                                    a[21].value == 0 then
                                      return "forward"
                                else
                                      return false
                                end
                          	end
               },
               forwardRight = State:create
               { method = 	function()
				   				--print("subright");
								robot.wheels.set_velocity(5,0)
                                a = robot.proximity
                                if  a[19].value ~= 0 and
                                    a[21].value == 0 then
                                      return "forward"
                                else
                                      return false
                                end
                           end
			   },
            },
            method = 	function()
							--print("subentry");
							robot.wheels.set_velocity(5,5)
						end
         },
   },
   transitions =
   {
      {condition =   function ()
                        a = robot.proximity
                        if a[1].value ~= 0 or a[22].value~=0 or
                           a[2].value ~= 0 or a[23].value~=0 or
                           a[3].value ~= 0 or a[24].value~=0 then
                           return true
                        else
                           return false
                        end
                     end,
            from = "goStraight", to = "turnLeft"},
      {condition =   function ()
                        a = robot.proximity
                        if a[1].value ~= 0 or a[22].value~=0 or
                           a[2].value ~= 0 or a[23].value~=0 or
                           a[3].value ~= 0 or a[24].value~=0 then
                           return false
                        else
                           return true
                        end
                     end,
            from = "turnLeft", to = "goAlongWall"},
      ---[[
      {condition =   function ()
                        a = robot.proximity
                        if a[1].value ~= 0 or a[22].value~=0 or
                           a[2].value ~= 0 or a[23].value~=0 or
                           a[3].value ~= 0 or a[24].value~=0 then
                           return true
                        else
                           return false
                        end
                     end,
            from = "goAlongWall", to = "turnLeft"},
   }
}

function init()
   reset()
end

function step()
   statemachine:stepSingle()
   --[[
   robot.leds.set_single_color(counter+1, "red")
   counter = (counter + 1) %
   a = robot.proximity
   if a[1].value ~= 0 or a[24].value~=0 or
      a[2].value ~= 0 or a[23].value~=0 then
	   robot.wheels.set_velocity(-5,5)
   else
	   robot.wheels.set_velocity(5,5)
   end

   robot.leds.set_single_color(counter+1, "green")
   --]]
end

function reset()
   counter = 0
   robot.leds.set_all_colors("red")
	-- robot.colored_blob_omnidirectional_camera.enable()
	--robot.wheels.set_velocity(5,5)
end

function destroy()
end