Add menu test nxt and oc programs

menu_programs/cube_gyro_acc.py

+'''
+Example program for showing gyroscope and accelerometer values from the ICM20608-G sensor
+'''
+from time import sleep
+
+from lib.robot_consts import Button, Sensor
+
+
+def main():
+    global robot
+    
+    robot.init_sensor(sensor_type=Sensor.GYRO_ACC)
+
+    while True:
+        # Read and print gyroscope and accelerometer values
+        gyro_acc_meas = robot.sensors.gyro_acc.read_value()
+        robot.display.fill(0)
+        robot.display.centered_text("Gyro Acc", 0, 1)
+        robot.display.text('Acc x y z:', 0, 10, 1)
+        robot.display.text(f"{gyro_acc_meas[3]}, {gyro_acc_meas[4]}, {gyro_acc_meas[5]}", 0, 18, 1)
+        robot.display.text('Gyro x y z:', 0, 16, 1)
+        robot.display.text(f"{gyro_acc_meas[0]}, {gyro_acc_meas[1]}, {gyro_acc_meas[2]}", 0, 34, 1)
+        print("Acc:", gyro_acc_meas[0], gyro_acc_meas[1], gyro_acc_meas[2],
+              "Gyro:",gyro_acc_meas[3], gyro_acc_meas[4], gyro_acc_meas[5])
+        
+        # Exit program if left cube button is pressed
+        buttons = robot.buttons.pressed()
+        if buttons[Button.LEFT]:
+            robot.deinit_all()
+            break
+        sleep(0.1)
+
+main()
\ No newline at end of file

menu_programs/cube_utility.py

+'''
+Example program for cube utility testing
+The program measures battery voltage, turns on and off buzzer and LED
+and displays info on the display
+'''
+from time import sleep
+
+from lib.robot_consts import Button
+
+def main():
+    global robot
+
+    counter = 0
+    while True:
+        counter += 1
+        # Turn on or off buzzer and LED 
+        if counter % 2 == 0:
+            robot.buzzer.set_freq_duty(1000, 0.02)
+            robot.led.on()
+        else:
+            robot.buzzer.off()
+            robot.led.off()
+
+        # Get battery voltage and print it
+        bat_voltage = robot.battery.voltage()
+        print("Battery voltage: ", bat_voltage)
+
+        # Show info on the display
+        robot.display.fill(0)
+        robot.display.text('Counter: {}'.format(counter), 0, 30, 1)
+        robot.display.text('Bat: {} V'.format(bat_voltage), 87, 0, 1)
+        robot.display.text('LED state: {}'.format(counter % 2 == 0), 0, 40, 1)
+        robot.display.show()
+
+        # Get cube buttons states and print it
+        buttons = robot.buttons.pressed()
+        print("Buttons state:", buttons)
+        # Exit program if left cube button is pressed
+        if buttons[Button.LEFT]:
+            break
+        sleep(0.5)
+
+    robot.deinit_all()
+
+main()
\ No newline at end of file

menu_programs/motor.py

+'''
+Example program for NXT or EV3 motors on a chosen port
+'''
+from time import sleep
+import struct
+import utime
+
+from lib.robot_consts import Button, Port
+
+MAX_MOTOR_POWER = 100
+MOTOR_POWER_INC = 20
+
+def main():
+    global robot, chosen_motor_port
+    
+    # Initialize motors with encoders
+    robot.init_motor(chosen_motor_port)
+    robot.motors[chosen_motor_port].init_encoder()
+    pwr = 0
+    debounce = False
+
+    counter = 0
+    while(True):
+        counter+=1
+        robot.motors[chosen_motor_port].set_power(pwr)
+        # Get encoder positions and speeds and print them
+        pos1 = robot.motors[chosen_motor_port].position()
+        speed1 = robot.motors[chosen_motor_port].speed()
+        print("Enc pos:", pos1,"Speed:", speed1)
+        robot.display.fill(0)
+        robot.display.centered_text(f"Motor M{chosen_motor_port+1}", 0, 1)
+        robot.display.text(f"Power: {pwr}", 0, 10, 1)
+        robot.displat.text(f"Pos: {pos1}", 0, 18, 1)
+        robot.display.text(f"Speed: {speed1}", 0, 26, 1)
+
+        # Exit program if left cube button is pressed
+        buttons = robot.buttons.pressed()
+        if buttons[Button.LEFT]:
+            robot.deinit_all()
+            break
+        elif not debounce:
+            if buttons[Button.UP]:
+                pwr += MOTOR_POWER_INC
+                if pwr > MAX_MOTOR_POWER:
+                    pwr = MAX_MOTOR_POWER
+            elif buttons[Button.DOWN]:
+                pwr -= MOTOR_POWER_INC
+                if pwr < -MAX_MOTOR_POWER:
+                    pwr = -MAX_MOTOR_POWER
+        if not buttons[Button.UP] and not buttons[Button.DOWN]:
+            debounce = False
+        sleep(0.1)
+main()
+  

menu_programs/nxt_light.py

+'''
+Example program for the NXT light sensors on chosen port
+The program measures the light intensity with the LED on and off
+'''
+from time import sleep
+
+from lib.robot_consts import Button, Port, Sensor
+
+
+def main():
+    global robot, chosen_sensor_port
+
+    robot.init_sensor(Sensor.NXT_LIGHT, chosen_sensor_port)
+
+    value_on, value_off = 0
+    counter = 0
+    while True:
+        counter += 1 
+
+        # Change light sensor LED state
+        if counter % 2:
+            state = 'on'
+            robot.sensors.light[chosen_sensor_port].on()
+            value_on = robot.sensors.light[chosen_sensor_port].get_value(0)
+        else:
+            state = 'off'
+            robot.sensors.light[chosen_sensor_port].off()
+            value_off = robot.sensors.light[chosen_sensor_port].get_value(0)
+
+        robot.display.fill(0)
+        robot.display.centered_text(f"NXT Light S{chosen_sensor_port}", 0, 1)
+        robot.display.text(f"LED state: {state}", 0, 10, 1)
+        robot.display.text(f"Intensity on: {value_on}", 0, 18, 1)
+        robot.display.text(f"Intensity off: {value_off}", 0, 26, 1)
+        robot.display.show()
+
+        print("LED state", state, "Light intensity on:",value_on, "off:", value_off)
+        
+        # Exit program if left cube button is pressed
+        buttons = robot.buttons.pressed()
+        if buttons[Button.LEFT]:
+            robot.deinit_all()
+            break
+        sleep(0.5)
+
+main()
\ No newline at end of file

menu_programs/nxt_sound.py

+'''
+Example program for the NXT sound sensors on chosen port
+The program measures the sound intensity
+'''
+from time import sleep
+
+from lib.robot_consts import Button, Port, Sensor
+
+
+def main():
+    global robot, chosen_sensor_port
+
+    # Initialize sound sensors on chosen port
+    robot.init_sensor(Sensor.NXT_SOUND, chosen_sensor_port)
+    while True:
+        # Get sound intensity and print it
+        intensity = robot.sensors.sound[chosen_sensor_port].intensity()
+        
+        robot.display.fill(0)
+        robot.display.centered_text(f"NXT Sound S{chosen_sensor_port}", 0, 1)
+        robot.display.text(f"Intensity: {intensity}", 0, 18, 1)
+        print(f"Sound level: {intensity}")
+
+        # Exit program if left cube button is pressed
+        buttons = robot.buttons.pressed()
+        if buttons[Button.LEFT]:
+            break
+        sleep(0.1)
+
+    robot.deinit_all()
+
+main()
\ No newline at end of file

menu_programs/nxt_touch.py

+'''
+Example program for the NXT touch sensors on chosen port
+The program measures touch sensor state
+'''
+from time import sleep
+
+from lib.robot_consts import Button, Port, Sensor
+
+
+def main():
+    global robot, chosen_sensor_port
+
+    # Initialize sound sensors on chosen port
+    robot.init_sensor(Sensor.NXT_TOUCH, chosen_sensor_port)
+    while True:
+        touch_pressed = robot.sensors.touch[chosen_sensor_port].pressed()
+        
+        robot.display.fill(0)
+        robot.display.centered_text(f"NXT Touch S{chosen_sensor_port}", 0, 1)
+        robot.display.text(f"Intensity: {touch_pressed}", 0, 18, 1)
+        print(f"Presed: {"True" if touch_pressed else "False"}")
+
+        # Exit program if left cube button is pressed
+        buttons = robot.buttons.pressed()
+        if buttons[Button.LEFT]:
+            break
+        sleep(1)
+
+    robot.deinit_all()
+
+main()
\ No newline at end of file

menu_programs/nxt_ultra.py

+'''
+Example program for the NXT ultrasonic sensor
+'''
+from time import sleep
+
+from lib.robot_consts import Button, Sensor
+
+
+def main():
+    global robot
+    # Initialize the ultrasonic sensor
+    robot.init_sensor(sensor_type=Sensor.NXT_ULTRASONIC)
+    distance = 0
+
+    while True:
+        # Get distance measurement and print it
+        distance = robot.sensors.ultra_nxt.distance()
+
+        robot.display.fill(0)
+        robot.display.centered_text("NXT Ultrasonic", 0, 1)
+        robot.display.text(f'Distance: {distance}', 0, 16, 1)
+        robot.display.show()
+        
+        print("Distance: ", distance)
+
+        # Exit program if left cube button is pressed
+        buttons = robot.buttons.pressed()
+        if buttons[Button.LEFT]:
+            robot.deinit_all()
+            break
+        sleep(0.1)
+
+main()
\ No newline at end of file

menu_programs/oc_color.py

+'''
+Example program for the Open-Cube Color sensor.
+Measure and show light intensity on display.
+Pressing down button changes sensor mode. (Reflection Red, Ambient, Color, 
+Red reflection raw, RGB raw, Green reflection raw, Blue Reflection raw)
+'''
+import utime
+
+from lib.OC import ColorSensor
+from lib.robot_consts import Port, Button, Sensor
+
+mode_str = (("Reflection"), ("Ambient"), ("Color"), ("RGB raw"), 
+            ("R Reflection raw"), ("G Reflection raw"), ("B Reflection raw"))
+N_MODES = len(mode_str)
+
+def main():
+    global robot, chosen_sensor_port
+
+    robot.display.fill(0)
+    robot.display.text("Connecting...", 0, 0, 1)
+    robot.display.show()
+    robot.init_sensor(sensor_type=Sensor.OC_COLOR, port=chosen_sensor_port)
+
+
+    mode = 0
+    debounce = False
+    while True:
+        robot.display.fill(0)
+        robot.display.centered_text(f"OC Color S{chosen_sensor_port}", 0, 1)
+        robot.display.text(mode_str[mode], 0, 10, 1)
+        if mode == 0:
+            ref = robot.sensors.light[chosen_sensor_port].reflection()
+            robot.display.text("R: {}".format(ref), 0, 18, 1)
+        if mode == 1:
+            ambient = robot.sensors.light[chosen_sensor_port].ambient()
+            robot.display.text("A: {}".format(ambient), 0, 18, 1)
+        if mode == 2:
+            color = robot.sensors.light[chosen_sensor_port].color()
+            robot.display.text("C: {}".format(color), 0, 18, 1)
+        if mode == 4:
+            ref, off = robot.sensors.light[chosen_sensor_port].reflection_raw()
+            robot.display.text("R: {}".format(ref), 0, 18, 1)
+            robot.display.text("Off: {}".format(off), 0, 26, 1)
+        if mode == 3:
+            r, g, b, off = robot.sensors.light[chosen_sensor_port].rgb_raw()
+            robot.display.text("R: {}".format(r), 0, 18, 1)
+            robot.display.text("G: {}".format(g), 0, 26, 1)
+            robot.display.text("B: {}".format(b), 0, 34, 1)
+            robot.display.text("Off: {}".format(off), 0, 42, 1)
+        if mode == 5:
+            ref, off = robot.sensors.light[chosen_sensor_port].reflection_raw_green()
+            robot.display.text("G: {}".format(ref), 0, 18, 1)
+            robot.display.text("Off: {}".format(off), 0, 26, 1)
+        if mode == 6:
+            ref, off = robot.sensors.light[chosen_sensor_port].reflection_raw_blue()
+            robot.display.text("B: {}".format(ref), 0, 18, 1)
+            robot.display.text("Off: {}".format(off), 0, 26, 1)
+        robot.display.show()
+
+        pressed = robot.buttons.pressed()
+        # Exit program if left cube button is pressed
+        if pressed[Button.LEFT]:
+            break
+        if not debounce:
+            if pressed[Button.DOWN]:
+                mode += 1
+                mode %= N_MODES
+                debounce = True
+                print("Mode:", mode_str[mode])
+            elif pressed[Button.UP]:
+                mode -= 1
+                mode %= N_MODES
+                debounce = True
+                print("Mode:", mode_str[mode])
+            
+        if not pressed[Button.DOWN] and not pressed[Button.UP]:
+            debounce = False
+        utime.sleep(0.1)
+        
+
+
+main()

menu_programs/oc_gyro.py

+'''
+Example program for the Open-Cube Gyro sensor on chosen port.
+Pressing down button changes sensor mode. (Euler angles, Quaternions, Raw values, Calibration)
+'''
+from re import M
+import sys
+import utime
+
+from lib.robot_consts import Button, Port, Sensor
+
+mode_str = (("Euler angles"), ("Quaternions"), ("Raw values"), ("Gyro Calib"), ("Mag Calib"))
+N_MODES = len(mode_str)
+
+def main():
+    global robot, chosen_sensor_port
+    robot.init_sensor(Sensor.OC_GYRO, chosen_sensor_port)
+
+    robot.display.fill(0)
+    robot.display.text("Connecting...", 0, 0, 1)
+    robot.display.show()
+
+    mode = 0
+    debounce = False
+    while True:
+        robot.display.fill(0)
+        robot.display.centered_text(f"OC Gyro S{chosen_sensor_port}", 0, 1)
+        robot.display.text(mode_str[mode], 0, 10, 1)
+
+        if mode == 0:
+            euler = robot.sensors.gyro[chosen_sensor_port].euler_angles()
+            robot.display.text("yaw:   {} deg".format(euler[0]), 0, 18, 1)
+            robot.display.text("pitch: {} deg".format(euler[1]), 0, 26, 1)
+            robot.display.text("roll:  {} deg".format(euler[2]), 0, 34, 1)
+        if mode == 1:
+            quat = robot.sensors.gyro[chosen_sensor_port].quaternions()
+            robot.display.text("{},{}".format(quat[0], quat[1]), 0, 18, 1)
+            robot.display.text("{},{}".format(quat[2], quat[3]), 0, 26, 1)
+        if mode == 2:
+            raw = robot.sensors.gyro[chosen_sensor_port].raw()
+            robot.display.text("g{},{},{}".format(raw[0][0], raw[0][1], raw[0][2],), 0, 18, 1)
+            robot.display.text("a{},{},{}".format(raw[1][0], raw[1][1], raw[1][2],), 0, 26, 1)
+            robot.display.text("m{},{},{}".format(raw[2][0], raw[2][1], raw[2][2],), 0, 34, 1)
+        if mode == 3:
+            cal = robot.sensors.gyro[chosen_sensor_port].calibrate_gyro()
+            robot.display.text("Calibrated: {}".format(cal), 0, 18, 1)
+        if mode == 3:
+            cal = robot.sensors.gyro[chosen_sensor_port].calibrate_mag()
+            robot.display.text("Calibrated: {}".format(cal), 0, 18, 1)
+
+
+        
+        robot.display.show()
+        
+        pressed = robot.buttons.pressed()
+        # Exit program if left cube button is pressed
+        if pressed[Button.LEFT]:
+            break
+        if not debounce:
+            if pressed[Button.DOWN]:
+                mode += 1
+                mode %= N_MODES
+                debounce = True
+                print("Mode:", mode_str[mode])
+            elif pressed[Button.UP]:
+                mode -= 1
+                mode %= N_MODES
+                debounce = True
+                print("Mode:", mode_str[mode])
+            
+        if not pressed[Button.DOWN] and not pressed[Button.UP]:
+            debounce = False
+        utime.sleep(0.1)
+        
+
+main()

menu_programs/oc_laser.py

+'''
+Example program for the Open-Cube Laser sensor o port 1.
+Measure and show distance on display.
+Pressing down button changes sensor mode. (Normal, Large FOV, Short distance)
+'''
+import utime
+
+from lib.robot_consts import Port, Button, Sensor
+
+mode_str = (("Normal"), ("Large FOV"), ("Short dist"))
+N_MODES = len(mode_str)
+LED_DIST = 200
+
+def main():
+    global robot, chosen_sensor_port
+
+    robot.display.fill(0)
+    robot.display.text("Connecting...", 0, 0, 1)
+    robot.display.show()
+
+    robot.init_sensor(sensor_type=Sensor.OC_LASER, port=chosen_sensor_port)
+    # At this distance (and closer) sensor green LED lights up (200 mm)
+    robot.sensors.laser[chosen_sensor_port].set_led_distance()
+    mode = 0
+    
+    debounce = False
+    while True:
+        if mode == 0:
+            dist = robot.sensors.laser[chosen_sensor_port].distance()
+        if mode == 1:
+            dist = robot.sensors.laser[chosen_sensor_port].distance_fov()
+        if mode == 2:
+            dist = robot.sensors.laser[chosen_sensor_port].distance_short()
+       
+        robot.display.fill(0)
+        robot.display.centered_text(f"OC Laser S{chosen_sensor_port}", 0, 1)
+        robot.display.text(mode_str[mode], 0, 10, 1)
+        robot.display.text(f"LED dist: {LED_DIST} mm", 0, 18, 1)
+        robot.display.text(f"Real dist: {dist} mm", 0, 26, 1)
+        robot.display.show()
+        
+        pressed = robot.buttons.pressed()
+        # Exit program if left cube button is pressed
+        if pressed[Button.LEFT]:
+            break;
+        if not debounce:
+            if pressed[Button.DOWN]:
+                mode += 1
+                mode %= N_MODES
+                debounce = True
+                print("Mode:", mode)
+            if pressed[Button.UP]:
+                mode -= 1
+                mode %= N_MODES
+                debounce = True
+                print("Mode:", mode)
+            
+        if not pressed[Button.DOWN] and not pressed[Button.UP]:
+            debounce = False
+        utime.sleep(0.1)
+        
+
+main()
\ No newline at end of file

menu_programs/oc_ultra.py

+'''
+Example program for the Open-Cube Ultrasonic sensor
+Measure and show distance on display.
+'''
+import utime
+
+from lib.robot_consts import Port, Button, Sensor
+
+LED_DIST = 200
+
+def main():
+    global robot, chosen_sensor_port
+
+    robot.display.fill(0)
+    robot.display.text("Connecting...", 0, 0, 1)
+    robot.display.show()
+
+    robot.init_sensor(sensor_type=Sensor.OC_ULTRASONIC, port=chosen_sensor_port)
+    # At this distance (and closer) sensor green LED lights up (200 mm)
+    robot.sensors.ultrasonic[chosen_sensor_port].set_led_distance(LED_DIST)
+
+    while True:
+        dist = robot.sensors.ultrasonic[chosen_sensor_port].distance()
+        robot.display.fill(0)
+        robot.display.centered_text(f"OC Ultrasonic S{chosen_sensor_port}", 0, 1)
+        #robot.display.text(mode_str[mode], 0, 10, 1)
+        robot.display.text(f"LED dist: {LED_DIST} mm", 0, 18, 1)
+        robot.display.text(f"Real dist: {dist} mm", 0, 26, 1)
+        robot.display.show()
+        
+        pressed = robot.buttons.pressed()
+        # Exit program if left cube button is pressed
+        if pressed[Button.LEFT]:
+            break
+        utime.sleep(0.1)
+
+main()
\ No newline at end of file