LFR/Controller.vdmrt at master · bandurvp/LFR · 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
class Controller

instance variables
	servoLeft: RobotServo;
	servoRight: RobotServo;
	sensorRightVal :RobotSensor;
	sensorLeftVal :RobotSensor;

operations

	public Controller : RobotServo * RobotServo * RobotSensor * RobotSensor ==> Controller
	Controller(servL, servR, sensRightVal, sensLeftVal) ==
	(
	  servoLeft:= servL;
  	servoRight:= servR;
	  sensorRightVal:= sensRightVal;
  	sensorLeftVal:= sensLeftVal
  );

   private control_loop : () ==> ()
	control_loop() == cycles(0) (

		if (sensorRightVal.getReading() < 150 and sensorLeftVal.getReading() < 150)
		then (
			servoRight.setServo(-HardwareInterface`forwardSpeed.getValue());
			servoLeft.setServo(HardwareInterface`forwardSpeed.getValue());
		);

		if (sensorRightVal.getReading() < 150 and sensorLeftVal.getReading() > 150)
		then (
			servoRight.setServo(-(HardwareInterface`forwardRotate.getValue()));
			servoLeft.setServo(HardwareInterface`backwardRotate.getValue());
		);

		if (sensorRightVal.getReading() > 150 and sensorLeftVal.getReading() < 150)
		then (
			servoRight.setServo(-(HardwareInterface`backwardRotate.getValue()));
			servoLeft.setServo(HardwareInterface`forwardRotate.getValue());
		);

		if (sensorRightVal.getReading() > 150 and sensorLeftVal.getReading() > 150)
		then skip
   );

thread

	periodic(10E6,0,0,0)(control_loop);

end Controller