diff --git a/TurboPi/HiwonderSDK/BuzzerControlDemo.py b/TurboPi/HiwonderSDK/BuzzerControlDemo.py new file mode 100644 index 0000000000000000000000000000000000000000..2d805973ec0c0b4b2c54e2e7a824625dd82f069c --- /dev/null +++ b/TurboPi/HiwonderSDK/BuzzerControlDemo.py @@ -0,0 +1,21 @@ +import time +import ros_robot_controller_sdk as rrc + +print(''' +********************************************************** +********功能:å¹»å°”ç§‘æŠ€æ ‘èŽ“æ´¾æ‰©å±•æ¿ï¼Œèœ‚鸣器控制例程(function: Hiwonder Raspberry Pi expansion board, buzzer control routine)********* +********************************************************** +---------------------------------------------------------- +Official website:https://www.hiwonder.com +Online mall:https://hiwonder.tmall.com +---------------------------------------------------------- +Tips: + * 按下Ctrl+Cå¯å…³é—æ¤æ¬¡ç¨‹åºè¿è¡Œï¼Œè‹¥å¤±è´¥è¯·å¤šæ¬¡å°è¯•ï¼(press Ctrl+C to close the running program, please try multiple times if failed) +---------------------------------------------------------- +''') +board = rrc.Board() +board.set_buzzer(1900, 0.1, 0.9, 1) # 以1900Hz的频率,æŒç»å“0.1秒,关é—0.9秒,é‡å¤1次(at a frequency of 1900Hz, continuously sound for 0.1 seconds, then silence for 0.9 seconds, repeat once) +time.sleep(2) +board.set_buzzer(1000, 0.5, 0.5, 0) # 以1000Hz的频率,æŒç»å“0.5秒,关é—0.5秒,一直é‡å¤(at a frequency of 1000Hz, continuously sound for 0.5 seconds, then silence for 0.5 seconds, repeat once) +time.sleep(3) +board.set_buzzer(1000, 0.0, 0.0, 1) # å…³é—(close) diff --git a/TurboPi/HiwonderSDK/FourInfrared.py b/TurboPi/HiwonderSDK/FourInfrared.py new file mode 100644 index 0000000000000000000000000000000000000000..ea48206ec697f1fb8f7b736bb50e9efe172aeae4 --- /dev/null +++ b/TurboPi/HiwonderSDK/FourInfrared.py @@ -0,0 +1,27 @@ +#!/usr/bin/python3 +# coding=utf8 +import time +import smbus + +#å››è·¯å·¡çº¿ä¼ æ„Ÿå™¨ä½¿ç”¨ä¾‹ç¨‹(4-channel line patrol sensor usage routine) + +class FourInfrared: + + def __init__(self, address=0x78, bus=1): + self.address = address + self.bus = smbus.SMBus(bus) + + def readData(self, register=0x01): + value = self.bus.read_byte_data(self.address, register) + return [True if value & v > 0 else False for v in [0x01, 0x02, 0x04, 0x08]] + + +if __name__ == "__main__": + line = FourInfrared() + while True: + data = line.readData() + #True表示识别到黑线,False表示没有识别到黑线(True means recognize the black line, False means does not recognize the black line) + print("Sensor1:", data[0], " Sensor2:", data[1], " Sensor3:", data[2], " Sensor4:", data[3]) + time.sleep(0.5) + + diff --git a/TurboPi/HiwonderSDK/Misc.py b/TurboPi/HiwonderSDK/Misc.py new file mode 100644 index 0000000000000000000000000000000000000000..57ac40988abda32f3221a602b234ddd11b4ecfb6 --- /dev/null +++ b/TurboPi/HiwonderSDK/Misc.py @@ -0,0 +1,12 @@ +#!/usr/bin/env python3 + +def map(x, in_min, in_max, out_min, out_max): + return (x - in_min) * (out_max - out_min) / (in_max - in_min) + out_min + +def emptyFunc(img = None): + return img + +def setRange(x, x_min, x_max): + tmp = x if x > x_min else x_min + tmp = tmp if tmp < x_max else x_max + return tmp diff --git a/TurboPi/HiwonderSDK/MotorControlDemo.py b/TurboPi/HiwonderSDK/MotorControlDemo.py new file mode 100644 index 0000000000000000000000000000000000000000..310199489221fc6f076c301a32c1a320ace8230d --- /dev/null +++ b/TurboPi/HiwonderSDK/MotorControlDemo.py @@ -0,0 +1,52 @@ +#!/usr/bin/python3 +# coding=utf8 +import sys +sys.path.append('/home/pi/TurboPi/') +import time +import signal +import ros_robot_controller_sdk as rrc + +if sys.version_info.major == 2: + print('Please run this program with python3!') + sys.exit(0) + +print(''' +********************************************************** +********功能:å¹»å°”ç§‘æŠ€æ ‘èŽ“æ´¾æ‰©å±•æ¿ï¼Œç”µæœºæŽ§åˆ¶ä¾‹ç¨‹(function: Hiwonder Raspberry Pi expansion board, motor control routine)********** +********************************************************** +---------------------------------------------------------- +Official website:https://www.hiwonder.com +Online mall:https://hiwonder.tmall.com +---------------------------------------------------------- +Tips: + * 按下Ctrl+Cå¯å…³é—æ¤æ¬¡ç¨‹åºè¿è¡Œï¼Œè‹¥å¤±è´¥è¯·å¤šæ¬¡å°è¯•ï¼(press Ctrl+C to close the running program, please try multiple times if failed) +---------------------------------------------------------- +''') +board = rrc.Board() + +start = True +#å…³é—å‰å¤„ç†(process program before closing) +def Stop(signum, frame): + global start + + start = False + print('å…³é—ä¸...') + board.set_motor_duty([[1, 0], [2, 0], [3, 0], [4, 0]]) # 关闿‰€æœ‰ç”µæœº(close all motors) + +signal.signal(signal.SIGINT, Stop) + +if __name__ == '__main__': + + while True: + board.set_motor_duty([[1, 35]]) #设置1å·ç”µæœºé€Ÿåº¦35(set motor 1 speed to 35) + time.sleep(0.2) + board.set_motor_duty([[1, 90]]) #设置1å·ç”µæœºé€Ÿåº¦90(set motor 1 speed to 90) + time.sleep(0.2) + + if not start: + board.set_motor_duty([[1, 0], [2, 0], [3, 0], [4, 0]]) # 关闿‰€æœ‰ç”µæœº(close all motors) + print('已关é—') + break + + + diff --git a/TurboPi/HiwonderSDK/PID.py b/TurboPi/HiwonderSDK/PID.py new file mode 100644 index 0000000000000000000000000000000000000000..cfe782b6d75c03030adc1db25921080b26439b18 --- /dev/null +++ b/TurboPi/HiwonderSDK/PID.py @@ -0,0 +1,111 @@ +"""Ivmech PID Controller is simple implementation of a Proportional-Integral-Derivative (PID) Controller in the Python Programming Language. +More information about PID Controller: http://en.wikipedia.org/wiki/PID_controller +""" +import sys +import time + +if sys.version_info.major == 2: + print('Please run this program with python3!') + sys.exit(0) + +class PID: + """PID Controller + """ + + def __init__(self, P=0.2, I=0.0, D=0.0): + + self.Kp = P + self.Ki = I + self.Kd = D + + self.sample_time = 0.00 + self.current_time = time.time() + self.last_time = self.current_time + + self.clear() + + def clear(self): + """Clears PID computations and coefficients""" + self.SetPoint = 0.0 + + self.PTerm = 0.0 + self.ITerm = 0.0 + self.DTerm = 0.0 + self.last_error = 0.0 + + # Windup Guard + self.int_error = 0.0 + self.windup_guard = 20.0 + + self.output = 0.0 + + def update(self, feedback_value): + """Calculates PID value for given reference feedback + .. math:: + u(t) = K_p e(t) + K_i \int_{0}^{t} e(t)dt + K_d {de}/{dt} + .. figure:: images/pid_1.png + :align: center + Test PID with Kp=1.2, Ki=1, Kd=0.001 (test_pid.py) + """ + error = self.SetPoint - feedback_value + + self.current_time = time.time() + delta_time = self.current_time - self.last_time + delta_error = error - self.last_error + + if (delta_time >= self.sample_time): + self.PTerm = self.Kp * error + self.ITerm += error * delta_time + + if (self.ITerm < -self.windup_guard): + self.ITerm = -self.windup_guard + elif (self.ITerm > self.windup_guard): + self.ITerm = self.windup_guard + + self.DTerm = 0.0 + if delta_time > 0: + self.DTerm = delta_error / delta_time + + # Remember last time and last error for next calculation + self.last_time = self.current_time + self.last_error = error + + self.output = self.PTerm + (self.Ki * self.ITerm) + (self.Kd * self.DTerm) + + def setKp(self, proportional_gain): + """Determines how aggressively the PID reacts to the current error with setting Proportional Gain""" + self.Kp = proportional_gain + + def setKi(self, integral_gain): + """Determines how aggressively the PID reacts to the current error with setting Integral Gain""" + self.Ki = integral_gain + + def setKd(self, derivative_gain): + """Determines how aggressively the PID reacts to the current error with setting Derivative Gain""" + self.Kd = derivative_gain + + def setWindup(self, windup): + """Integral windup, also known as integrator windup or reset windup, + refers to the situation in a PID feedback controller where + a large change in setpoint occurs (say a positive change) + and the integral terms accumulates a significant error + during the rise (windup), thus overshooting and continuing + to increase as this accumulated error is unwound + (offset by errors in the other direction). + The specific problem is the excess overshooting. + """ + self.windup_guard = windup + + def setSampleTime(self, sample_time): + """PID that should be updated at a regular interval. + Based on a pre-determined sampe time, the PID decides if it should compute or return immediately. + """ + self.sample_time = sample_time + + +if __name__ == '__main__': + x_pid = PID(P=0.2, I=0, D=0) + x_pid.SetPoint = 5 + x_pid.update(10) + out = x_pid.output + print (out) diff --git a/TurboPi/HiwonderSDK/PWMServoControlDemo.py b/TurboPi/HiwonderSDK/PWMServoControlDemo.py new file mode 100644 index 0000000000000000000000000000000000000000..e13599b4c5cbdf713c339666985c11b131857558 --- /dev/null +++ b/TurboPi/HiwonderSDK/PWMServoControlDemo.py @@ -0,0 +1,56 @@ +#!/usr/bin/python3 +# coding=utf8 +import sys +sys.path.append('/home/pi/TurboPi/') +import time +import signal +import threading +import ros_robot_controller_sdk as rrc + +if sys.version_info.major == 2: + print('Please run this program with python3!') + sys.exit(0) + +print(''' +********************************************************** +********功能:å¹»å°”ç§‘æŠ€æ ‘èŽ“æ´¾æ‰©å±•æ¿ï¼ŒPWM舵机控制例程(function: Hiwonder Raspberry Pi expansion board, PWM servo control routine)********** +********************************************************** +---------------------------------------------------------- +Official website:https://www.hiwonder.com +Online mall:https://hiwonder.tmall.com +---------------------------------------------------------- +Tips: + * 按下Ctrl+Cå¯å…³é—æ¤æ¬¡ç¨‹åºè¿è¡Œï¼Œè‹¥å¤±è´¥è¯·å¤šæ¬¡å°è¯•ï¼(press Ctrl+C to close the running program, please try multiple times if failed) +---------------------------------------------------------- +''') + +board = rrc.Board() +start = True +#å…³é—å‰å¤„ç†(process program before closing) +def Stop(signum, frame): + global start + + start = False + print('å…³é—ä¸...') + +signal.signal(signal.SIGINT, Stop) + +if __name__ == '__main__': + + while True: + board.pwm_servo_set_position(1, [[1, 1100]]) # 设置1å·èˆµæœºè„‰å®½ä¸º2500,è¿è¡Œæ—¶é—´ä¸º1000毫秒(set servo 1 pulse width to 2500, running time to 1000 milliseconds) + time.sleep(1) + board.pwm_servo_set_position(1, [[1, 1500]]) # 设置1å·èˆµæœºè„‰å®½ä¸º1500,è¿è¡Œæ—¶é—´ä¸º1000毫秒(set servo 1 pulse width to 1500, running time to 1000 milliseconds) + time.sleep(1) + board.pwm_servo_set_position(1, [[1, 1900], [2, 1000]]) #设置1å·èˆµæœºè„‰å®½ä¸º1000,设置2å·èˆµæœºè„‰å®½ä¸º1000, è¿è¡Œæ—¶é—´ä¸º1000毫秒(set servo 1 pulse width to 1000, set servo 2 pulse width to 1000) + time.sleep(1) + board.pwm_servo_set_position(1, [[1, 1500], [2, 2000]]) #设置1å·èˆµæœºè„‰å®½ä¸º2000,设置2å·èˆµæœºè„‰å®½ä¸º2000, è¿è¡Œæ—¶é—´ä¸º1000毫秒(set servo 1 pulse width to 2000, set servo 2 pulse width to 2000, running time to 1000 milliseconds) + time.sleep(1) + if not start: + board.pwm_servo_set_position(1, [[1, 1500], [2, 1500]]) # 设置1å·èˆµæœºè„‰å®½ä¸º1500,设置2å·èˆµæœºè„‰å®½ä¸º1500, è¿è¡Œæ—¶é—´ä¸º1000毫秒(set servo 1 pulse width to 1500, set servo 2 pulse width to 1500, running time to 1000 milliseconds) + time.sleep(1) + print('已关é—') + break + + + diff --git a/TurboPi/HiwonderSDK/RGBControlDemo.py b/TurboPi/HiwonderSDK/RGBControlDemo.py new file mode 100644 index 0000000000000000000000000000000000000000..008c5e33e69768469412a4d67567bbff4335c61b --- /dev/null +++ b/TurboPi/HiwonderSDK/RGBControlDemo.py @@ -0,0 +1,51 @@ +import time +import signal +import ros_robot_controller_sdk as rrc + +print(''' +********************************************************** +********功能:å¹»å°”ç§‘æŠ€æ ‘èŽ“æ´¾æ‰©å±•æ¿ï¼ŒRGBç¯æŽ§åˆ¶ä¾‹ç¨‹(function: Hiwonder Raspberry Pi expansion board, RGB light control routine)********** +********************************************************** +---------------------------------------------------------- +Official website:https://www.hiwonder.com +Online mall:https://hiwonder.tmall.com +---------------------------------------------------------- +Tips: + * 按下Ctrl+Cå¯å…³é—æ¤æ¬¡ç¨‹åºè¿è¡Œï¼Œè‹¥å¤±è´¥è¯·å¤šæ¬¡å°è¯•ï¼(press Ctrl+C to close the running program, please try multiple times if failed) +---------------------------------------------------------- +''') + +start = True +#å…³é—å‰å¤„ç†(process program before closing) +def Stop(signum, frame): + global start + + start = False + print('å…³é—ä¸...') +board = rrc.Board() +#先将所有ç¯å…³é—(turn all lights off firstly) +board.set_rgb([[1, 0, 0, 0], [2, 0, 0, 0]]) +signal.signal(signal.SIGINT, Stop) + +while True: + #设置2个ç¯ä¸ºçº¢è‰²(set two lights to red) + board.set_rgb([[1, 255, 0, 0], [2, 255, 0, 0]]) + time.sleep(1) + + #设置2个ç¯ä¸ºç»¿è‰²(set two lights to green) + board.set_rgb([[1, 0, 255, 0], [2, 0, 255, 0]]) + time.sleep(1) + + #设置2个ç¯ä¸ºè“色(set two lights to blue) + board.set_rgb([[1, 0, 0, 255], [2, 0, 0, 255]]) + time.sleep(1) + + #设置2个ç¯ä¸ºé»„色(set two lights to yellow) + board.set_rgb([[1, 255, 255, 0], [2, 255, 255, 0]]) + time.sleep(1) + + if not start: + #所有ç¯å…³é—(turn all lights off) + board.set_rgb([[1, 0, 0, 0], [2, 0, 0, 0]]) + print('已关é—') + break diff --git a/TurboPi/HiwonderSDK/Sonar.py b/TurboPi/HiwonderSDK/Sonar.py new file mode 100644 index 0000000000000000000000000000000000000000..3a839d8da6bad4ebf01406e52ffb200674ad38ea --- /dev/null +++ b/TurboPi/HiwonderSDK/Sonar.py @@ -0,0 +1,137 @@ +import sys +import time +from smbus2 import SMBus, i2c_msg + +# 幻尔科技iic超声波库(Hiwonder I2C ultrasonic library) + +if sys.version_info.major == 2: + print('Please run this program with python3!') + sys.exit(0) + +class Sonar: + __units = {"mm":0, "cm":1} + __dist_reg = 0 + + __RGB_MODE = 2 + __RGB1_R = 3 + __RGB1_G = 4 + __RGB1_B = 5 + __RGB2_R = 6 + __RGB2_G = 7 + __RGB2_B = 8 + + __RGB1_R_BREATHING_CYCLE = 9 + __RGB1_G_BREATHING_CYCLE = 10 + __RGB1_B_BREATHING_CYCLE = 11 + __RGB2_R_BREATHING_CYCLE = 12 + __RGB2_G_BREATHING_CYCLE = 13 + __RGB2_B_BREATHING_CYCLE = 14 + def __init__(self): + self.i2c_addr = 0x77 + self.i2c = 1 + self.Pixels = [0,0] + self.RGBMode = 0 + + def __getattr(self, attr): + if attr in self.__units: + return self.__units[attr] + if attr == "Distance": + return self.getDistance() + else: + raise AttributeError('Unknow attribute : %s'%attr) + + def setRGBMode(self, mode): + try: + with SMBus(self.i2c) as bus: + bus.write_byte_data(self.i2c_addr, self.__RGB_MODE, mode) + except BaseException as e: + print(e) + + def show(self): #å ä½ï¼Œä¸Žæ‰©å±•æ¿RGBä¿æŒè°ƒç”¨ä¸€è‡´(occupied, consistent with the call to the expansion board RGB) + pass + + def numPixels(self): + return 2 + + def setPixelColor(self, index, rgb): + color = (rgb[0] << 16) | (rgb[1] << 8) | rgb[2] + try: + if index != 0 and index != 1: + return + start_reg = 3 if index == 0 else 6 + with SMBus(self.i2c) as bus: + bus.write_byte_data(self.i2c_addr, start_reg, 0xFF & (color >> 16)) + bus.write_byte_data(self.i2c_addr, start_reg+1, 0xFF & (color >> 8)) + bus.write_byte_data(self.i2c_addr, start_reg+2, 0xFF & color) + self.Pixels[index] = color + except BaseException as e: + print(e) + + def getPixelColor(self, index): + if index != 0 and index != 1: + raise ValueError("Invalid pixel index", index) + return ((self.Pixels[index] >> 16) & 0xFF, + (self.Pixels[index] >> 8) & 0xFF, + self.Pixels[index] & 0xFF) + + def setBreathCycle(self, index, rgb, cycle): + try: + if index != 0 and index != 1: + return + if rgb < 0 or rgb > 2: + return + start_reg = 9 if index == 0 else 12 + cycle = int(cycle / 100) + with SMBus(self.i2c) as bus: + bus.write_byte_data(self.i2c_addr, start_reg + rgb, cycle) + except BaseException as e: + print(e) + + def startSymphony(self): + self.setRGBMode(1) + self.setBreathCycle(1,0, 2000) + self.setBreathCycle(1,1, 3300) + self.setBreathCycle(1,2, 4700) + self.setBreathCycle(2,0, 4600) + self.setBreathCycle(2,1, 2000) + self.setBreathCycle(2,2, 3400) + + def getDistance(self): + dist = 99999 + try: + with SMBus(self.i2c) as bus: + msg = i2c_msg.write(self.i2c_addr, [0,]) + bus.i2c_rdwr(msg) + read = i2c_msg.read(self.i2c_addr, 2) + bus.i2c_rdwr(read) + dist = int.from_bytes(bytes(list(read)), byteorder='little', signed=False) + if dist > 5000: + dist = 5000 + except BaseException as e: + print(e) + return dist + +if __name__ == '__main__': + s = Sonar() + s.setRGBMode(0) + s.setPixelColor(0, (0, 0, 0)) + s.setPixelColor(1, (0, 0, 0)) + s.show() + time.sleep(0.1) + s.setPixelColor(0, (255, 0, 0)) + s.setPixelColor(1, (255, 0, 0)) + s.show() + time.sleep(1) + s.setPixelColor(0, (0, 255, 0)) + s.setPixelColor(1, (0, 255, 0)) + s.show() + time.sleep(1) + s.setPixelColor(0, (0, 0, 255)) + s.setPixelColor(1, (0, 0, 255)) + s.show() + time.sleep(1) + s.startSymphony() + while True: + time.sleep(0.1) + print(s.getDistance()) + diff --git a/TurboPi/HiwonderSDK/hardware_test.py b/TurboPi/HiwonderSDK/hardware_test.py new file mode 100644 index 0000000000000000000000000000000000000000..5e646685f4f89f72b006135b146d8592d1466082 --- /dev/null +++ b/TurboPi/HiwonderSDK/hardware_test.py @@ -0,0 +1,62 @@ +#!/usr/bin/python3 +# coding=utf8 +import sys +import time +sys.path.append('/home/pi/TurboPi/') +import HiwonderSDK.ros_robot_controller_sdk as rrc + +if sys.version_info.major == 2: + print('Please run this program with python3!') + sys.exit(0) + +print(''' +********************************************************** +********************PWM舵机和电机测试(PWM servo and motor test)************************ +********************************************************** +---------------------------------------------------------- +Official website:https://www.hiwonder.com +Online mall:https://hiwonder.tmall.com +---------------------------------------------------------- +Tips: + * 按下Ctrl+Cå¯å…³é—æ¤æ¬¡ç¨‹åºè¿è¡Œï¼Œè‹¥å¤±è´¥è¯·å¤šæ¬¡å°è¯•ï¼(press Ctrl+C to close the running program, please try multiple times if failed) +---------------------------------------------------------- +''') +board = rrc.Board() +board.pwm_servo_set_position(0.3, [[1, 1800]]) +time.sleep(0.3) +board.pwm_servo_set_position(0.3, [[1, 1500]]) +time.sleep(0.3) +board.pwm_servo_set_position(0.3, [[1, 1200]]) +time.sleep(0.3) +board.pwm_servo_set_position(0.3, [[1, 1500]]) +time.sleep(1.5) + +board.pwm_servo_set_position(0.3, [[2, 1200]]) +time.sleep(0.3) +board.pwm_servo_set_position(0.3, [[2, 1500]]) +time.sleep(0.3) +board.pwm_servo_set_position(0.3, [[2, 1800]]) +time.sleep(0.3) +board.pwm_servo_set_position(0.3, [[2, 1500]]) +time.sleep(1.5) + +board.set_motor_duty([[1, -45]]) +time.sleep(0.5) +board.set_motor_duty([[1, 0]]) +time.sleep(1) + +board.set_motor_duty([[2, 45]]) +time.sleep(0.5) +board.set_motor_duty([[2, 0]]) +time.sleep(1) + +board.set_motor_duty([[3, -45]]) +time.sleep(0.5) +board.set_motor_duty([[3, 0]]) +time.sleep(1) + +board.set_motor_duty([[4, 45]]) +time.sleep(0.5) +board.set_motor_duty([[4, 0]]) +time.sleep(1) + diff --git a/TurboPi/HiwonderSDK/key.py b/TurboPi/HiwonderSDK/key.py new file mode 100644 index 0000000000000000000000000000000000000000..ef321c61e9e42bbeaa397414d04bfbef6d389a71 --- /dev/null +++ b/TurboPi/HiwonderSDK/key.py @@ -0,0 +1,21 @@ +#!/usr/bin/env python3 +# encoding:utf-8 +import time +import gpiod + +try: + key1_pin = 13 + chip = gpiod.Chip("gpiochip4") + key1 = chip.get_line(key1_pin) + key1.request(consumer="key1", type=gpiod.LINE_REQ_DIR_IN, flags=gpiod.LINE_REQ_FLAG_BIAS_PULL_UP) + + key2_pin = 23 + key2 = chip.get_line(key2_pin) + key2.request(consumer="key2", type=gpiod.LINE_REQ_DIR_IN, flags=gpiod.LINE_REQ_FLAG_BIAS_PULL_UP) + while True: + print('\rkey1: {} key2: {}'.format(key1.get_value(), key2.get_value()), end='', flush=True) # 打å°key状æ€(press key state) + time.sleep(0.001) + chip.close() +except: + print('按键默认被hw_button_scanå 用,需è¦å…ˆå…³é—æœåŠ¡') + print('sudo systemctl stop hw_button_scan.service') diff --git a/TurboPi/HiwonderSDK/led.py b/TurboPi/HiwonderSDK/led.py new file mode 100644 index 0000000000000000000000000000000000000000..05fdc697cb016689e325d223e4295e944c74cd3f --- /dev/null +++ b/TurboPi/HiwonderSDK/led.py @@ -0,0 +1,26 @@ +#!/usr/bin/env python3 +# encoding:utf-8 +import time +import gpiod +try: + print('ledé—ªçƒ 0.1/s') + led1_pin = 16 # è“色led(blue led) + led2_pin = 26 + chip = gpiod.Chip('gpiochip4') + + led1 = chip.get_line(led1_pin) + led1.request(consumer="led1", type=gpiod.LINE_REQ_DIR_OUT) + + led2 = chip.get_line(led2_pin) + led2.request(consumer="led2", type=gpiod.LINE_REQ_DIR_OUT) + + while True: + led1.set_value(0) + led2.set_value(0) + time.sleep(0.1) + led1.set_value(1) + led2.set_value(1) + time.sleep(0.1) +except: + print('led默认被hw_wifiå 用,需è¦è‡ªè¡Œæ³¨é‡ŠæŽ‰ç›¸å…³ä»£ç ') + print('ç„¶åŽé‡å¯æœåŠ¡sudo systemctl restart hw_wifi.service') diff --git a/TurboPi/HiwonderSDK/mecanum.py b/TurboPi/HiwonderSDK/mecanum.py new file mode 100644 index 0000000000000000000000000000000000000000..1e4dc636da41b238fe068ce294bfef436937748c --- /dev/null +++ b/TurboPi/HiwonderSDK/mecanum.py @@ -0,0 +1,76 @@ +#!/usr/bin/python3 +# coding=utf8 +import sys +sys.path.append('/home/pi/TurboPi/') +import math +import HiwonderSDK.ros_robot_controller_sdk as rrc + +board = rrc.Board() + +class MecanumChassis: + # A = 67 # mm + # B = 59 # mm + # WHEEL_DIAMETER = 65 # mm + + def __init__(self, a=67, b=59, wheel_diameter=65): + self.a = a + self.b = b + self.wheel_diameter = wheel_diameter + self.velocity = 0 + self.direction = 0 + self.angular_rate = 0 + + def reset_motors(self): + board.set_motor_duty([[1, 0], [2, 0], [3, 0], [4, 0]]) + + self.velocity = 0 + self.direction = 0 + self.angular_rate = 0 + + def set_velocity(self, velocity, direction, angular_rate, fake=False): + """ + Use polar coordinates to control moving + motor1 v1| ↑ |v2 motor2 + | | + motor3 v3| |v4 motor4 + :param velocity: mm/s + :param direction: Moving direction 0~360deg, 180deg<--- ↑ ---> 0deg + :param angular_rate: The speed at which the chassis rotates + :param fake: + :return: + """ + rad_per_deg = math.pi / 180 + vx = velocity * math.cos(direction * rad_per_deg) + vy = velocity * math.sin(direction * rad_per_deg) + vp = -angular_rate * (self.a + self.b) + v1 = int(vy + vx - vp) + v2 = int(vy - vx + vp) + v3 = int(vy - vx - vp) + v4 = int(vy + vx + vp) + if fake: + return + board.set_motor_duty([[1, -v1], [2, v2], [3, -v3], [4, v4]]) + self.velocity = velocity + self.direction = direction + self.angular_rate = angular_rate + + def translation(self, velocity_x, velocity_y, fake=False): + velocity = math.sqrt(velocity_x ** 2 + velocity_y ** 2) + if velocity_x == 0: + direction = 90 if velocity_y >= 0 else 270 # pi/2 90deg, (pi * 3) / 2 270deg + else: + if velocity_y == 0: + direction = 0 if velocity_x > 0 else 180 + else: + direction = math.atan(velocity_y / velocity_x) # θ=arctan(y/x) (x!=0) + direction = direction * 180 / math.pi + if velocity_x < 0: + direction += 180 + else: + if velocity_y < 0: + direction += 360 + if fake: + return velocity, direction + else: + return self.set_velocity(velocity, direction, 0) + diff --git a/TurboPi/HiwonderSDK/ros_robot_controller_sdk.py b/TurboPi/HiwonderSDK/ros_robot_controller_sdk.py new file mode 100644 index 0000000000000000000000000000000000000000..0c51647bfb39823d7cf965fa0595afedc84e188a --- /dev/null +++ b/TurboPi/HiwonderSDK/ros_robot_controller_sdk.py @@ -0,0 +1,604 @@ +#!/usr/bin/env python3 +# encoding: utf-8 +# stm32 python sdk +import enum +import time +import copy +import queue +import struct +import serial +import threading + +class PacketControllerState(enum.IntEnum): + # 通信åè®®çš„æ ¼å¼(communication protocol format) + # 0xAA 0x55 Length Function ID Data Checksum + PACKET_CONTROLLER_STATE_STARTBYTE1 = 0 + PACKET_CONTROLLER_STATE_STARTBYTE2 = 1 + PACKET_CONTROLLER_STATE_LENGTH = 2 + PACKET_CONTROLLER_STATE_FUNCTION = 3 + PACKET_CONTROLLER_STATE_ID = 4 + PACKET_CONTROLLER_STATE_DATA = 5 + PACKET_CONTROLLER_STATE_CHECKSUM = 6 + +class PacketFunction(enum.IntEnum): + # å¯é€šè¿‡ä¸²å£å®žçŽ°çš„æŽ§åˆ¶åŠŸèƒ½(control functions achievable via serial port.) + PACKET_FUNC_SYS = 0 + PACKET_FUNC_LED = 1 # LED控制(LED control) + PACKET_FUNC_BUZZER = 2 # 蜂鸣器控制(buzzer control) + PACKET_FUNC_MOTOR = 3 # 电机控制(motor control) + PACKET_FUNC_PWM_SERVO = 4 # PWM舵机控制, æ¿åä¸Šä»Žé‡Œåˆ°å¤–ä¾æ¬¡ä¸º1-4(PWM servo control, the board is numbered 1 to 4 from inside to outside) + PACKET_FUNC_BUS_SERVO = 5 # 总线舵机控制(bus servo control) + PACKET_FUNC_KEY = 6 # 按键获å–(key obtain) + PACKET_FUNC_IMU = 7 # IMU获å–(IMU obtain) + PACKET_FUNC_GAMEPAD = 8 # 手柄获å–(handle grip obtain) + PACKET_FUNC_SBUS = 9 # èˆªæ¨¡é¥æŽ§èŽ·å–(remote control for aircraft model obtain) + PACKET_FUNC_OLED = 10 # OLED 显示内容设置(OLED display content setting) + PACKET_FUNC_RGB = 11 # 设置RGB颜色(set RGB color) + PACKET_FUNC_NONE = 12 + +class PacketReportKeyEvents(enum.IntEnum): + # 按键的ä¸åŒçжæ€(different states of key) + KEY_EVENT_PRESSED = 0x01 + KEY_EVENT_LONGPRESS = 0x02 + KEY_EVENT_LONGPRESS_REPEAT = 0x04 + KEY_EVENT_RELEASE_FROM_LP = 0x08 + KEY_EVENT_RELEASE_FROM_SP = 0x10 + KEY_EVENT_CLICK = 0x20 + KEY_EVENT_DOUBLE_CLICK= 0x40 + KEY_EVENT_TRIPLE_CLICK = 0x80 + +crc8_table = [ + 0, 94, 188, 226, 97, 63, 221, 131, 194, 156, 126, 32, 163, 253, 31, 65, + 157, 195, 33, 127, 252, 162, 64, 30, 95, 1, 227, 189, 62, 96, 130, 220, + 35, 125, 159, 193, 66, 28, 254, 160, 225, 191, 93, 3, 128, 222, 60, 98, + 190, 224, 2, 92, 223, 129, 99, 61, 124, 34, 192, 158, 29, 67, 161, 255, + 70, 24, 250, 164, 39, 121, 155, 197, 132, 218, 56, 102, 229, 187, 89, 7, + 219, 133, 103, 57, 186, 228, 6, 88, 25, 71, 165, 251, 120, 38, 196, 154, + 101, 59, 217, 135, 4, 90, 184, 230, 167, 249, 27, 69, 198, 152, 122, 36, + 248, 166, 68, 26, 153, 199, 37, 123, 58, 100, 134, 216, 91, 5, 231, 185, + 140, 210, 48, 110, 237, 179, 81, 15, 78, 16, 242, 172, 47, 113, 147, 205, + 17, 79, 173, 243, 112, 46, 204, 146, 211, 141, 111, 49, 178, 236, 14, 80, + 175, 241, 19, 77, 206, 144, 114, 44, 109, 51, 209, 143, 12, 82, 176, 238, + 50, 108, 142, 208, 83, 13, 239, 177, 240, 174, 76, 18, 145, 207, 45, 115, + 202, 148, 118, 40, 171, 245, 23, 73, 8, 86, 180, 234, 105, 55, 213, 139, + 87, 9, 235, 181, 54, 104, 138, 212, 149, 203, 41, 119, 244, 170, 72, 22, + 233, 183, 85, 11, 136, 214, 52, 106, 43, 117, 151, 201, 74, 20, 246, 168, + 116, 42, 200, 150, 21, 75, 169, 247, 182, 232, 10, 84, 215, 137, 107, 53 +] + +def checksum_crc8(data): + # æ ¡éªŒ(calibration) + check = 0 + for b in data: + check = crc8_table[check ^ b] + return check & 0x00FF + +class SBusStatus: + def __init__(self): + self.channels = [0] * 16; + self.channel_17 = False + self.channel_18 = False + self.signal_loss = True + self.fail_safe = False + +class Board: + buttons_map = { + 'GAMEPAD_BUTTON_MASK_L2': 0x0001, + 'GAMEPAD_BUTTON_MASK_R2': 0x0002, + 'GAMEPAD_BUTTON_MASK_SELECT': 0x0004, + 'GAMEPAD_BUTTON_MASK_START': 0x0008, + 'GAMEPAD_BUTTON_MASK_L3': 0x0020, + 'GAMEPAD_BUTTON_MASK_R3': 0x0040, + 'GAMEPAD_BUTTON_MASK_CROSS': 0x0100, + 'GAMEPAD_BUTTON_MASK_CIRCLE': 0x0200, + 'GAMEPAD_BUTTON_MASK_SQUARE': 0x0800, + 'GAMEPAD_BUTTON_MASK_TRIANGLE': 0x1000, + 'GAMEPAD_BUTTON_MASK_L1': 0x4000, + 'GAMEPAD_BUTTON_MASK_R1': 0x8000 + } + + def __init__(self, device="/dev/ttyAMA0", baudrate=1000000, timeout=5): + self.enable_recv = False + self.frame = [] + self.recv_count = 0 + + self.port = serial.Serial(None, baudrate, timeout=timeout) + self.port.rts = False + self.port.dtr = False + self.port.setPort(device) + self.port.open() + + self.state = PacketControllerState.PACKET_CONTROLLER_STATE_STARTBYTE1 + self.servo_read_lock = threading.Lock() + self.pwm_servo_read_lock = threading.Lock() + + self.sys_queue = queue.Queue(maxsize=1) + self.bus_servo_queue = queue.Queue(maxsize=1) + self.pwm_servo_queue = queue.Queue(maxsize=1) + self.key_queue = queue.Queue(maxsize=1) + self.imu_queue = queue.Queue(maxsize=1) + self.gamepad_queue = queue.Queue(maxsize=1) + self.sbus_queue = queue.Queue(maxsize=1) + + self.parsers = { + PacketFunction.PACKET_FUNC_SYS: self.packet_report_sys, + PacketFunction.PACKET_FUNC_KEY: self.packet_report_key, + PacketFunction.PACKET_FUNC_IMU: self.packet_report_imu, + PacketFunction.PACKET_FUNC_GAMEPAD: self.packet_report_gamepad, + PacketFunction.PACKET_FUNC_BUS_SERVO: self.packet_report_serial_servo, + PacketFunction.PACKET_FUNC_SBUS: self.packet_report_sbus, + PacketFunction.PACKET_FUNC_PWM_SERVO: self.packet_report_pwm_servo + } + + threading.Thread(target=self.recv_task, daemon=True).start() + time.sleep(0.1) + + def packet_report_sys(self, data): + try: + self.sys_queue.put_nowait(data) + except queue.Full: + pass + + def packet_report_key(self, data): + try: + self.key_queue.put_nowait(data) + except queue.Full: + pass + + def packet_report_imu(self, data): + try: + self.imu_queue.put_nowait(data) + except queue.Full: + pass + + def packet_report_gamepad(self, data): + try: + self.gamepad_queue.put_nowait(data) + except queue.Full: + pass + + def packet_report_serial_servo(self, data): + try: + self.bus_servo_queue.put_nowait(data) + except queue.Full: + pass + + def packet_report_pwm_servo(self, data): + try: + self.pwm_servo_queue.put_nowait(data) + except queue.Full: + pass + + def packet_report_sbus(self, data): + try: + self.sbus_queue.put_nowait(data) + except queue.Full: + pass + + def get_battery(self): + if self.enable_recv: + try: + data = self.sys_queue.get(block=False) + if data[0] == 0x04: + return struct.unpack('<H', data[1:])[0] + else: + None + except queue.Empty: + return None + else: + # print('enable reception first!') + return None + + def get_button(self): + if self.enable_recv: + try: + data = self.key_queue.get(block=False) + key_id = data[0] + key_event = PacketReportKeyEvents(data[1]) + if key_event == PacketReportKeyEvents.KEY_EVENT_CLICK: + return key_id, 0 + elif key_event == PacketReportKeyEvents.KEY_EVENT_PRESSED: + return key_id, 1 + except queue.Empty: + return None + else: + # print('enable reception first!') + return None + + def get_imu(self): + if self.enable_recv: + try: + # ax, ay, az, gx, gy, gz + return struct.unpack('<6f', self.imu_queue.get(block=False)) + except queue.Empty: + return None + else: + # print('enable reception first!') + return None + + def get_gamepad(self): + if self.enable_recv: + try: + # buttons, hat, lx, ly, rx, ry + gamepad_data = struct.unpack("<HB4b", self.gamepad_queue.get(block=False)) + # 'lx', 'ly', 'rx', 'ry', 'r2', 'l2', 'hat_x', 'hat_y' + axes = [0, 0, 0, 0, 0, 0, 0, 0] + # 'cross', 'circle', '', 'square', 'triangle', '', 'l1', 'r1', 'l2', 'r2', 'select', 'start', '', 'l3', 'r3', '' + buttons = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] + for b in self.buttons_map: + if self.buttons_map[b] & gamepad_data[0]: + if b == 'GAMEPAD_BUTTON_MASK_R2': + axes[4] = 1 + elif b == 'GAMEPAD_BUTTON_MASK_L2': + axes[5] = 1 + elif b == 'GAMEPAD_BUTTON_MASK_CROSS': + buttons[0] = 1 + elif b == 'GAMEPAD_BUTTON_MASK_CIRCLE': + buttons[1] = 1 + elif b == 'GAMEPAD_BUTTON_MASK_SQUARE': + buttons[3] = 1 + elif b == 'GAMEPAD_BUTTON_MASK_TRIANGLE': + buttons[4] = 1 + elif b == 'GAMEPAD_BUTTON_MASK_L1': + buttons[6] = 1 + elif b == 'GAMEPAD_BUTTON_MASK_R1': + buttons[7] = 1 + elif b == 'GAMEPAD_BUTTON_MASK_SELECT': + buttons[10] = 1 + elif b == 'GAMEPAD_BUTTON_MASK_START': + buttons[11] = 1 + + if gamepad_data[2] > 0: + axes[0] = -gamepad_data[2] / 127 + elif gamepad_data[2] < 0: + axes[0] = -gamepad_data[2] / 128 + + if gamepad_data[3] > 0: + axes[1] = gamepad_data[3] / 127 + elif gamepad_data[3] < 0: + axes[1] = gamepad_data[3] / 128 + + if gamepad_data[4] > 0: + axes[2] = -gamepad_data[4] / 127 + elif gamepad_data[4] < 0: + axes[2] = -gamepad_data[4] / 128 + + if gamepad_data[5] > 0: + axes[3] = gamepad_data[5] / 127 + elif gamepad_data[5] < 0: + axes[3] = gamepad_data[5] / 128 + + if gamepad_data[1] == 9: + axes[6] = 1 + elif gamepad_data[1] == 13: + axes[6] = -1 + + if gamepad_data[1] == 11: + axes[7] = -1 + elif gamepad_data[1] == 15: + axes[7] = 1 + return axes, buttons + except queue.Empty: + return None + else: + # print('enable reception first!') + return None + + def get_sbus(self): + if self.enable_recv: + try: + sbus_data = self.sbus_queue.get(block=False) + status = SBusStatus() + *status.channels, ch17, ch18, sig_loss, fail_safe = struct.unpack("<16hBBBB", sbus_data) + status.channel_17 = ch17 != 0 + status.channel_18 = ch18 != 0 + status.signal_loss = sig_loss != 0 + status.fail_safe = fail_safe != 0 + data = [] + if status.signal_loss: + data = 16 * [0.5] + data[4] = 0 + data[5] = 0 + data[6] = 0 + data[7] = 0 + else: + for i in status.channels: + data.append((i - 192)/(1792 - 192)) + return data + except queue.Empty: + return None + else: + # print('enable reception first!') + return None + + def buf_write(self, func, data): + buf = [0xAA, 0x55, int(func)] + buf.append(len(data)) + buf.extend(data) + buf.append(checksum_crc8(bytes(buf[2:]))) + self.port.write(buf) + + def set_led(self, on_time, off_time, repeat=1, led_id=1): + on_time = int(on_time*1000) + off_time = int(off_time*1000) + self.buf_write(PacketFunction.PACKET_FUNC_LED, struct.pack("<BHHH", led_id, on_time, off_time, repeat)) + + def set_buzzer(self, freq, on_time, off_time, repeat=1): + on_time = int(on_time*1000) + off_time = int(off_time*1000) + self.buf_write(PacketFunction.PACKET_FUNC_BUZZER, struct.pack("<HHHH", freq, on_time, off_time, repeat)) + + def set_motor_speed(self, speeds): + data = [0x01, len(speeds)] + for i in speeds: + data.extend(struct.pack("<Bf", int(i[0] - 1), float(i[1]))) + self.buf_write(PacketFunction.PACKET_FUNC_MOTOR, data) + + def set_oled_text(self, line, text): + data = [line, len(text)] # å命令为 0x01 设置 SSID, 第二个å—节是å—符串长度,该长度包å«'\0'å—符串结æŸç¬¦(sub-command is 0x01, set SSID, the second byte indicates the length of the string, which includes the '\0' string termination character) + data.extend(bytes(text, encoding='utf-8')) + self.buf_write(PacketFunction.PACKET_FUNC_OLED, data) + + def set_rgb(self, pixels): + data = [0x01, len(pixels), ] + for index, r, g, b in pixels: + data.extend(struct.pack("<BBBB", int(index - 1), int(r), int(g), int(b))) + self.buf_write(PacketFunction.PACKET_FUNC_RGB, data) + + def set_motor_duty(self, dutys): + data = [0x05, len(dutys)] + for i in dutys: + data.extend(struct.pack("<Bf", int(i[0] - 1), float(i[1]))) + self.buf_write(PacketFunction.PACKET_FUNC_MOTOR, data) + + def pwm_servo_set_position(self, duration, positions): + duration = int(duration * 1000) + data = [0x01, duration & 0xFF, 0xFF & (duration >> 8), len(positions)] + for i in positions: + data.extend(struct.pack("<BH", i[0], i[1])) + self.buf_write(PacketFunction.PACKET_FUNC_PWM_SERVO, data) + + def pwm_servo_set_offset(self, servo_id, offset): + data = struct.pack("<BBb", 0x07, servo_id, int(offset)) + self.buf_write(PacketFunction.PACKET_FUNC_PWM_SERVO, data) + + def pwm_servo_read_and_unpack(self, servo_id, cmd, unpack): + with self.servo_read_lock: + self.buf_write(PacketFunction.PACKET_FUNC_PWM_SERVO, [cmd, servo_id]) + data = self.pwm_servo_queue.get(block=True) + servo_id, cmd, info = struct.unpack(unpack, data) + return info + + def pwm_servo_read_offset(self, servo_id): + return self.pwm_servo_read_and_unpack(servo_id, 0x09, "<BBb") + + def pwm_servo_read_position(self, servo_id): + return self.pwm_servo_read_and_unpack(servo_id, 0x05, "<BBH") + + def bus_servo_enable_torque(self, servo_id, enable): + if enable: + data = struct.pack("<BB", 0x0B, servo_id) + else: + data = struct.pack("<BB", 0x0C, servo_id) + self.buf_write(PacketFunction.PACKET_FUNC_BUS_SERVO, data) + time.sleep(0.02) + + def bus_servo_set_id(self, servo_id_now, servo_id_new): + data = struct.pack("<BBB", 0x10, servo_id_now, servo_id_new) + self.buf_write(PacketFunction.PACKET_FUNC_BUS_SERVO, data) + time.sleep(0.02) + + def bus_servo_set_offset(self, servo_id, offset): + data = struct.pack("<BBb", 0x20, servo_id, int(offset)) + self.buf_write(PacketFunction.PACKET_FUNC_BUS_SERVO, data) + time.sleep(0.02) + + def bus_servo_save_offset(self, servo_id): + data = struct.pack("<BB", 0x24, servo_id) + self.buf_write(PacketFunction.PACKET_FUNC_BUS_SERVO, data) + time.sleep(0.02) + + def bus_servo_set_angle_limit(self, servo_id, limit): + data = struct.pack("<BBHH", 0x30, servo_id, int(limit[0]), int(limit[1])) + self.buf_write(PacketFunction.PACKET_FUNC_BUS_SERVO, data) + time.sleep(0.02) + + def bus_servo_set_vin_limit(self, servo_id, limit): + data = struct.pack("<BBHH", 0x34, servo_id, int(limit[0]), int(limit[1])) + self.buf_write(PacketFunction.PACKET_FUNC_BUS_SERVO, data) + time.sleep(0.02) + + def bus_servo_set_temp_limit(self, servo_id, limit): + data = struct.pack("<BBb", 0x38, servo_id, int(limit)) + self.buf_write(PacketFunction.PACKET_FUNC_BUS_SERVO, data) + time.sleep(0.02) + + def bus_servo_stop(self, servo_id): + data = [0x03, len(servo_id)] + data.extend(struct.pack("<"+'B'*len(servo_id), *servo_id)) + self.buf_write(PacketFunction.PACKET_FUNC_BUS_SERVO, data) + + def bus_servo_set_position(self, duration, positions): + duration = int(duration * 1000) + data = [0x01, duration & 0xFF, 0xFF & (duration >> 8), len(positions)] # 0x00 is bus servo sub command + for i in positions: + data.extend(struct.pack("<BH", i[0], i[1])) + self.buf_write(PacketFunction.PACKET_FUNC_BUS_SERVO, data) + + def bus_servo_read_and_unpack(self, servo_id, cmd, unpack): + with self.servo_read_lock: + self.buf_write(PacketFunction.PACKET_FUNC_BUS_SERVO, [cmd, servo_id]) + data = self.bus_servo_queue.get(block=True) + servo_id, cmd, success, *info = struct.unpack(unpack, data) + if success == 0: + return info + + def bus_servo_read_id(self, servo_id=254): + return self.bus_servo_read_and_unpack(servo_id, 0x12, "<BBbB") + + def bus_servo_read_offset(self, servo_id): + return self.bus_servo_read_and_unpack(servo_id, 0x22, "<BBbb") + + def bus_servo_read_position(self, servo_id): + return self.bus_servo_read_and_unpack(servo_id, 0x05, "<BBbh") + + def bus_servo_read_vin(self, servo_id): + return self.bus_servo_read_and_unpack(servo_id, 0x07, "<BBbH") + + def bus_servo_read_temp(self, servo_id): + return self.bus_servo_read_and_unpack(servo_id, 0x09, "<BBbB") + + def bus_servo_read_temp_limit(self, servo_id): + return self.bus_servo_read_and_unpack(servo_id, 0x3A, "<BBbB") + + def bus_servo_read_angle_limit(self, servo_id): + return self.bus_servo_read_and_unpack(servo_id, 0x32, "<BBb2H") + + def bus_servo_read_vin_limit(self, servo_id): + return self.bus_servo_read_and_unpack(servo_id, 0x36, "<BBb2H") + + def bus_servo_read_torque_state(self, servo_id): + return self.bus_servo_read_and_unpack(servo_id, 0x0D, "<BBbb") + + def enable_reception(self, enable=True): + self.enable_recv = enable + + def recv_task(self): + while True: + if self.enable_recv: + recv_data = self.port.read() + if recv_data: + for dat in recv_data: + # print("%0.2X "%dat) + if self.state == PacketControllerState.PACKET_CONTROLLER_STATE_STARTBYTE1: + if dat == 0xAA: + self.state = PacketControllerState.PACKET_CONTROLLER_STATE_STARTBYTE2 + continue + elif self.state == PacketControllerState.PACKET_CONTROLLER_STATE_STARTBYTE2: + if dat == 0x55: + self.state = PacketControllerState.PACKET_CONTROLLER_STATE_FUNCTION + else: + self.state = PacketControllerState.PACKET_CONTROLLER_STATE_STARTBYTE1 + continue + elif self.state == PacketControllerState.PACKET_CONTROLLER_STATE_FUNCTION: + if dat < int(PacketFunction.PACKET_FUNC_NONE): + self.frame = [dat, 0] + self.state = PacketControllerState.PACKET_CONTROLLER_STATE_LENGTH + else: + self.frame = [] + self.state = PacketControllerState.PACKET_CONTROLLER_STATE_STARTBYTE1 + continue + elif self.state == PacketControllerState.PACKET_CONTROLLER_STATE_LENGTH: + self.frame[1] = dat + self.recv_count = 0 + if dat == 0: + self.state = PacketControllerState.PACKET_CONTROLLER_STATE_CHECKSUM + else: + self.state = PacketControllerState.PACKET_CONTROLLER_STATE_DATA + continue + elif self.state == PacketControllerState.PACKET_CONTROLLER_STATE_DATA: + self.frame.append(dat) + self.recv_count += 1 + if self.recv_count >= self.frame[1]: + self.state = PacketControllerState.PACKET_CONTROLLER_STATE_CHECKSUM + continue + elif self.state == PacketControllerState.PACKET_CONTROLLER_STATE_CHECKSUM: + crc8 = checksum_crc8(bytes(self.frame)) + if crc8 == dat: + func = PacketFunction(self.frame[0]) + data = bytes(self.frame[2:]) + if func in self.parsers: + self.parsers[func](data) + else: + print("æ ¡éªŒå¤±è´¥") + self.state = PacketControllerState.PACKET_CONTROLLER_STATE_STARTBYTE1 + continue + else: + time.sleep(0.01) + self.port.close() + print("END...") + +def bus_servo_test(board): + board.bus_servo_set_position(1, [[1, 500], [2, 500]]) + time.sleep(1) + board.bus_servo_set_position(2, [[1, 0], [2, 0]]) + time.sleep(1) + board.bus_servo_stop([1, 2]) + time.sleep(1) + + servo_id = 1 + board.bus_servo_set_id(254, servo_id) + servo_id = board.bus_servo_read_id() + if servo_id is not None: + servo_id = servo_id[0] + + offset_set = -10 + board.bus_servo_set_offset(servo_id, offset_set) + board.bus_servo_save_offset(servo_id) + + vin_l, vin_h = 4500, 14500 + board.bus_servo_set_vin_limit(servo_id, [vin_l, vin_h]) + + temp_limit = 85 + board.bus_servo_set_temp_limit(servo_id, temp_limit) + + angle_l, angle_h = 0, 1000 + board.bus_servo_set_angle_limit(servo_id, [angle_l, angle_h]) + + board.bus_servo_enable_torque(servo_id, 1) + + print('id:', board.bus_servo_read_id(servo_id)) + print('offset:', board.bus_servo_read_offset(servo_id), offset_set) + print('vin:', board.bus_servo_read_vin(servo_id)) + print('temp:', board.bus_servo_read_temp(servo_id)) + print('position:', board.bus_servo_read_position(servo_id)) + print('angle_limit:', board.bus_servo_read_angle_limit(servo_id), [angle_l, angle_h]) + print('vin_limit:', board.bus_servo_read_vin_limit(servo_id), [vin_l, vin_h]) + print('temp_limit:', board.bus_servo_read_temp_limit(servo_id), temp_limit) + print('torque_state:', board.bus_servo_read_torque_state(servo_id)) + +def pwm_servo_test(board): + servo_id = 1 + board.pwm_servo_set_position(0.5, [[servo_id, 1500], [2, 1500]]) + board.pwm_servo_set_offset(servo_id, 0) + print('offset:', board.pwm_servo_read_offset(servo_id)) + print('position:', board.pwm_servo_read_position(servo_id)) + +if __name__ == "__main__": + board = Board() + board.enable_reception() + print("START...") + # board.set_led(0.1, 0.9, 1) + # board.set_buzzer(2400, 0.1, 0.9, 1) + board.set_motor_duty([[1, -50], [2, 50], [3, 50], [4, -50]]) + # board.set_motor_speed([[1, -0.3], [2, 0.3], [3, -0.3], [4, 0.3]]) + time.sleep(0.5) + # board.set_rgb([[1, 0, 0, 255]]) + board.set_motor_duty([[1, 0], [2, 0], [3, 0], [4, 0]]) + # board.set_motor_speed([[1, 0], [2, 0], [3, 0], [4, 0]]) + # bus_servo_test(board) + # pwm_servo_test(board) + # board.set_oled_text(1, "SSID:HW-ABC123") + # board.set_oled_text(2, "IP:192.168.149.1") + while True: + try: + # res = board.get_imu() + # if res is not None: + # print(res) + # res = board.get_button() + # if res is not None: + # print(res) + # data = board.get_gamepad() + # if data is not None: + # print(data[0]) + # print(data[1]) + # res = board.get_sbus() + # if res is not None: + # print(res) + res = board.get_battery() + if res is not None: + print(res) + time.sleep(0.001) + except KeyboardInterrupt: + break + diff --git a/backend/robot.py b/backend/robot.py new file mode 100644 index 0000000000000000000000000000000000000000..32172aa18ac97e5b858061f8ef357a23f31ee524 --- /dev/null +++ b/backend/robot.py @@ -0,0 +1,135 @@ +import sys + +sys.path.append("/home/pi/TurboPi/") + +import threading +import time +import cv2 +import serial +import HiwonderSDK.mecanum as mecanum +import HiwonderSDK.ros_robot_controller_sdk as rrc +import Camera + +state = {"battery": "N/A", "temperature": "N/A", "current_command": "stopped"} + +SPEED = 50 +ROTATION_SPEED = 0.2 + +car = mecanum.MecanumChassis() +board = None +camera = Camera.Camera() + +sensor_lock = threading.Lock() +board_lock = threading.Lock() + + +def initialize_board(): + global board + with board_lock: + try: + if board and board.port.is_open: + print("🔌 Closing existing serial port...") + board.port.close() + time.sleep(1) + + board = rrc.Board() + if not board.port.is_open: + board.port.open() + + board.enable_reception() + print("✅ Board initialized successfully") + + except serial.SerialException as e: + print(f"⌠Serial error during board initialization: {e}") + board = None + time.sleep(1) + except Exception as e: + print(f"⌠Failed to initialize board: {e}") + board = None + + +def update_sensors(): + global board + while True: + with sensor_lock: + if not board: + initialize_board() + + batt = None + retries = 5 + + while retries > 0 and batt is None: + try: + with board_lock: # <-- Lock serial port + if board.port.is_open: + batt = board.get_battery() + except serial.SerialException as e: + print(f"⌠Serial exception: {e}") + initialize_board() + time.sleep(0.5) + break + except Exception as e: + print(f"⌠Battery sensor error: {e}") + time.sleep(0.5) + + retries -= 1 + time.sleep(0.05) + + if batt is not None: + voltage = batt / 1000.0 + state["battery"] = f"{voltage:.1f}V" + else: + state["battery"] = "N/A" + + try: + with open("/sys/class/thermal/thermal_zone0/temp", "r") as f: + temp_str = f.read().strip() + temp_c = int(temp_str) / 1000.0 + state["temperature"] = f"{temp_c:.1f}°C" + except Exception as e: + print(f"⌠Temperature sensor error: {e}") + state["temperature"] = "N/A" + + time.sleep(5) + + +def stop_robot() -> None: + car.set_velocity(0, 0, 0) + state["current_command"] = "stopped" + print("🛑 Robot stopped.") + + +def move(direction: str) -> None: + if direction == "forward": + car.set_velocity(SPEED, 90, 0) + elif direction == "backward": + car.set_velocity(SPEED, 270, 0) + elif direction == "left": + car.set_velocity(SPEED, 0, 0) + elif direction == "right": + car.set_velocity(SPEED, 180, 0) + elif direction == "rotate_left": + car.set_velocity(0, 90, -ROTATION_SPEED) + elif direction == "rotate_right": + car.set_velocity(0, 90, ROTATION_SPEED) + else: + stop_robot() + return + state["current_command"] = direction + print(f"🚗 Moving {direction}") + + +def generate_frames(): + while True: + frame = camera.frame + if frame is not None: + ret, buffer = cv2.imencode(".jpg", frame) + if ret: + yield ( + b"--frame\r\n" + b"Content-Type: image/jpeg\r\n\r\n" + buffer.tobytes() + b"\r\n" + ) + time.sleep(0.05) + + +initialize_board() diff --git a/backend/server.py b/backend/server.py new file mode 100644 index 0000000000000000000000000000000000000000..135ca10d457ccfd96df8eca0cabdb7973d60e212 --- /dev/null +++ b/backend/server.py @@ -0,0 +1,105 @@ +import threading +import time +from flask import Flask, Response +from flask_cors import CORS +from flask_socketio import SocketIO, emit +import serial +from robot import ( + initialize_board, + move, + generate_frames, + camera, + update_sensors, + state, +) + +app = Flask(__name__) +CORS(app) +app.config["SECRET_KEY"] = "secret!" +socketio = SocketIO(app, cors_allowed_origins="*", async_mode="threading") +sensor_lock = threading.Lock() + +state["bluetooth_connected"] = False + + +def get_status_update(): + return { + "command": state["current_command"], + "sender": "server", + "battery": state["battery"], + "temperature": state["temperature"], + "bluetooth_connected": state["bluetooth_connected"], + } + + +@app.route("/") +def index(): + return "Try /video_feed or connect via WebSocket." + + +@app.route("/video_feed") +def video_feed(): + return Response( + generate_frames(), mimetype="multipart/x-mixed-replace; boundary=frame" + ) + + +@socketio.on("bluetooth_status") +def handle_bluetooth_status(data): + state["bluetooth_connected"] = data.get("connected", False) + print(f"Bluetooth connected: {state['bluetooth_connected']}") + emit("status_update", get_status_update(), broadcast=True) + + +@socketio.on("connect") +def on_connect(): + print("Client connected") + emit("status_update", get_status_update()) + + +@socketio.on("disconnect") +def on_disconnect(): + print("Bluetooth server disconnected") + state["bluetooth_connected"] = False + emit("status_update", get_status_update(), broadcast=True) + + +@socketio.on("move") +def on_move(data): + command = data.get("command") + if command: + move(command) + emit("status_update", get_status_update(), broadcast=True) + + +def emit_sensor_update_loop(): + while True: + try: + print(f"Battery: {state['battery']} | Temperature: {state['temperature']}") + socketio.emit("status_update", get_status_update()) + socketio.sleep(5) + except serial.SerialException as e: + print(f"⌠Serial error during sensor update: {e}") + initialize_board() + time.sleep(1) + except Exception as e: + print(f"⌠Sensor update error: {e}") + time.sleep(1) + + +if __name__ == "__main__": + threading.Thread(target=update_sensors, daemon=True).start() + threading.Thread( + target=lambda: camera.camera_open(correction=True), daemon=True + ).start() + socketio.start_background_task(emit_sensor_update_loop) + + print("Starting Flask server on 192.168.149.1:5000") + socketio.run( + app, + host="0.0.0.0", + port=5000, + debug=False, + use_reloader=False, + allow_unsafe_werkzeug=True, + ) diff --git a/bluetooth/bt_server.py b/bluetooth/bt_server.py new file mode 100644 index 0000000000000000000000000000000000000000..088c6cde42b93f03aa96b11e5754f58e625bfc36 --- /dev/null +++ b/bluetooth/bt_server.py @@ -0,0 +1,128 @@ +import sys + +sys.path.append("/home/pi/TurboPi/") + +import asyncio +from evdev import InputDevice, ecodes, list_devices +from socketio import Client +import lab2.backend.robot as robot + +sio = Client() + +connected = False +SERVER_URL = "http://192.168.149.1:5000" +GAMEPAD_DEVICE = "/dev/input/event8" + + +async def connect_to_server(): + retries = 5 + delay = 2 + + for attempt in range(retries): + try: + print( + f"🔌 Attempting to connect to Flask server... (try {attempt + 1}/{retries})" + ) + sio.connect(SERVER_URL) + print("✅ Connected to Flask server.") + return + except Exception as e: + print(f"⌠Connection attempt {attempt + 1} failed: {e}") + if attempt < retries - 1: + await asyncio.sleep(delay) + else: + print( + "🚨 Could not connect to Flask server. Please check if Flask is running." + ) + return + + +async def handle_input(event): + command = None + if event.type == ecodes.EV_KEY: + with robot.board_lock: + if event.code == ecodes.BTN_SOUTH and event.value == 1: # A Button + command = "forward" + robot.move(command) + elif event.code == ecodes.BTN_EAST and event.value == 1: # B Button + command = "backward" + robot.move(command) + elif event.code == ecodes.BTN_WEST and event.value == 1: # X Button + command = "rotate_right" + robot.move(command) + elif event.code == ecodes.BTN_NORTH and event.value == 1: # Y Button + command = "rotate_left" + robot.move(command) + elif event.code == ecodes.BTN_TL and event.value == 1: # Left bumper + command = "left" + robot.move(command) + elif event.code == ecodes.BTN_TR and event.value == 1: # Right bumper + command = "right" + robot.move(command) + elif event.value == 0: + command = "stop" + robot.stop_robot() + + if command: + await broadcast_state(command) + + +async def broadcast_state(command): + global connected + if sio.connected: + print(f"Broadcasting state: {command or 'no command'}") + sio.emit( + "bluetooth_status", + { + "connected": connected, + "command": command or robot.state["current_command"], + "battery": robot.state["battery"], + "temperature": robot.state["temperature"], + }, + ) + + +async def listen_controller(): + global connected + + try: + gamepad = InputDevice(GAMEPAD_DEVICE) + connected = True + print(f"🎮 Connected to {gamepad.name}") + await broadcast_state("connected") + + async for event in gamepad.async_read_loop(): + await handle_input(event) + + except OSError as e: + print(f"⌠Controller connection lost: {e}") + connected = False + await broadcast_state("disconnected") + await asyncio.sleep(2) + await try_reconnect_controller() + + +async def try_reconnect_controller(): + global connected + while not connected: + try: + devices = list_devices() + if GAMEPAD_DEVICE in devices: + print("✅ Reconnecting to controller...") + await listen_controller() + else: + print("🎮 Waiting for controller to reconnect...") + await asyncio.sleep(2) + except Exception as e: + print(f"⌠Error while reconnecting: {e}") + await asyncio.sleep(2) + + +async def main(): + await connect_to_server() + print("Starting Bluetooth server...") + await try_reconnect_controller() + + +if __name__ == "__main__": + asyncio.run(main()) diff --git a/frontend/src/renderer/src/components/CarController.tsx b/frontend/src/renderer/src/components/CarController.tsx index 22bb5dfae309e88c3c0c965c9248d1fbbb1123a0..3a170144b7d7ee7dde8eea3df3045adef46fb90d 100644 --- a/frontend/src/renderer/src/components/CarController.tsx +++ b/frontend/src/renderer/src/components/CarController.tsx @@ -91,7 +91,7 @@ export default function CarController(): JSX.Element { </div> <div className="flex-1 flex flex-row items-center overflow-hidden p-4 pt-0 gap-5"> - <LogViewer logs={logs} /> + <LogViewer logs={logs} setLogs={setLogs} /> <DirectionalControls onMove={handleMove} /> </div> </div> diff --git a/frontend/src/renderer/src/components/DirectionalControls.tsx b/frontend/src/renderer/src/components/DirectionalControls.tsx index 4af4231125f52e5b29a5c4640e2d5895fb2f2a99..95c946f410f89a3383e2a4cba033df6880b89811 100644 --- a/frontend/src/renderer/src/components/DirectionalControls.tsx +++ b/frontend/src/renderer/src/components/DirectionalControls.tsx @@ -1,4 +1,12 @@ -import { FaArrowUp, FaArrowDown, FaArrowLeft, FaArrowRight, FaUndo, FaRedo } from 'react-icons/fa' +import { + FaArrowUp, + FaArrowDown, + FaArrowLeft, + FaArrowRight, + FaUndo, + FaRedo, + FaStop +} from 'react-icons/fa' interface DirectionalControlsProps { onMove: (command: string) => void @@ -18,9 +26,12 @@ export default function DirectionalControls({ onMove }: DirectionalControlsProps </defs> </svg> - <div className="absolute w-48 h-48 bg-blue-200" style={{ clipPath: 'url(#myClippy)' }} /> + <div + className="absolute w-48 h-48 bg-blue-200" + style={{ clipPath: 'url(#myClippy)', top: '10%' }} + /> - <div className="absolute bg-blue-200 p-4 rounded-full" style={{ top: '-15%', left: '-5%' }}> + <div className="absolute bg-blue-200 p-4 rounded-full" style={{ top: '-14%', left: '-5%' }}> <button onClick={() => onMove('rotate_left')} className="relative z-10 w-10 h-10 bg-blue-400 text-white rounded-full flex items-center justify-center hover:bg-blue-600 active:scale-95 transition" @@ -29,7 +40,7 @@ export default function DirectionalControls({ onMove }: DirectionalControlsProps </button> </div> - <div className="absolute bg-blue-200 p-4 rounded-full" style={{ top: '-15%', right: '-5%' }}> + <div className="absolute bg-blue-200 p-4 rounded-full" style={{ top: '-14%', right: '-5%' }}> <button onClick={() => onMove('rotate_right')} className="relative z-10 w-10 h-10 bg-blue-400 text-white rounded-full flex items-center justify-center hover:bg-blue-600 active:scale-95 transition" @@ -40,15 +51,16 @@ export default function DirectionalControls({ onMove }: DirectionalControlsProps <button onClick={() => onMove('stop')} - className="z-10 w-12 h-12 bg-red-600 text-white font-bold text-xs flex items-center justify-center hover:bg-red-700 transition rounded-full" + className="absolute z-10 w-12 h-12 bg-red-500 text-white font-bold text-xs flex items-center justify-center hover:bg-red-600 transition rounded-full" + style={{ top: '48%', left: '50%', transform: 'translateX(-50%)' }} > - stop + <FaStop className="text-lg" /> </button> <button onClick={() => onMove('forward')} className="absolute z-10 w-10 h-10 bg-blue-400 text-blue-100 rounded-full flex items-center justify-center hover:bg-blue-600 active:scale-95 transition" - style={{ top: '8%', left: '50%', transform: 'translateX(-50%)' }} + style={{ top: '18%', left: '50%', transform: 'translateX(-50%)' }} > <FaArrowUp /> </button> @@ -56,7 +68,7 @@ export default function DirectionalControls({ onMove }: DirectionalControlsProps <button onClick={() => onMove('backward')} className="absolute z-10 w-10 h-10 bg-blue-400 text-blue-100 rounded-full flex items-center justify-center hover:bg-blue-600 active:scale-95 transition" - style={{ bottom: '8%', left: '50%', transform: 'translateX(-50%)' }} + style={{ bottom: '-3%', left: '50%', transform: 'translateX(-50%)' }} > <FaArrowDown /> </button> @@ -64,7 +76,7 @@ export default function DirectionalControls({ onMove }: DirectionalControlsProps <button onClick={() => onMove('left')} className="absolute z-10 w-10 h-10 bg-blue-400 text-blue-100 rounded-full flex items-center justify-center hover:bg-blue-600 active:scale-95 transition" - style={{ left: '8%', top: '50%', transform: 'translateY(-50%)' }} + style={{ left: '8%', top: '60%', transform: 'translateY(-50%)' }} > <FaArrowLeft /> </button> @@ -72,7 +84,7 @@ export default function DirectionalControls({ onMove }: DirectionalControlsProps <button onClick={() => onMove('right')} className="absolute z-10 w-10 h-10 bg-blue-400 text-blue-100 rounded-full flex items-center justify-center hover:bg-blue-600 active:scale-95 transition" - style={{ right: '8%', top: '50%', transform: 'translateY(-50%)' }} + style={{ right: '8%', top: '60%', transform: 'translateY(-50%)' }} > <FaArrowRight /> </button> diff --git a/frontend/src/renderer/src/components/LogViewer.tsx b/frontend/src/renderer/src/components/LogViewer.tsx index f58411e29e3d375764f74127862324b4702e6db7..8ab9d2fa82d5f0f52e856845dee72facfae489d6 100644 --- a/frontend/src/renderer/src/components/LogViewer.tsx +++ b/frontend/src/renderer/src/components/LogViewer.tsx @@ -1,11 +1,13 @@ import { useEffect, useRef } from 'react' import { LogEntry } from './CarController' +import { FaTrash } from 'react-icons/fa' interface LogViewerProps { logs: LogEntry[] + setLogs: (logs: LogEntry[]) => void } -export default function LogViewer({ logs }: LogViewerProps): JSX.Element { +export default function LogViewer({ logs, setLogs }: LogViewerProps): JSX.Element { const containerRef = useRef<HTMLDivElement>(null) useEffect(() => { @@ -14,19 +16,24 @@ export default function LogViewer({ logs }: LogViewerProps): JSX.Element { } }, [logs]) + function deleteLogs(): void { + setLogs([]) + } + return ( <div className="flex-1 flex flex-col gap-2 h-full"> - <div className="bg-blue-200 border border-blue-500 p-2 pl-3 rounded-lg"> + <div className="bg-blue-200 border border-blue-500 p-2 px-4 rounded-lg flex flex-row justify-between items-center"> <h2 className="font-bold text-blue-500">LOGS</h2> + <FaTrash className="text-blue-500 hover:text-blue-600" onClick={deleteLogs} /> </div> <div className="flex-1 bg-blue-200 border border-blue-500 rounded-lg p-4 flex flex-col min-h-0"> <div ref={containerRef} className="flex-1 overflow-y-auto text-sm rounded-lg"> <table className="w-full table-auto border-collapse"> <thead className="sticky top-0 bg-blue-400 text-blue-100"> <tr> - <th className="py-1 px-2 text-left">Time</th> - <th className="py-1 px-2 text-left">Sender</th> - <th className="py-1 px-2 text-left">Message</th> + <th className="py-2 px-2 text-left">Time</th> + <th className="py-2 px-2 text-left">Sender</th> + <th className="py-2 px-2 text-left">Message</th> </tr> </thead> diff --git a/frontend/src/renderer/src/components/SidePanel.tsx b/frontend/src/renderer/src/components/SidePanel.tsx index 38c472e289be2d02f202f3fbb6950c8416c00ff4..11e10030986b1399505d4e1951f64de58cd4a1c0 100644 --- a/frontend/src/renderer/src/components/SidePanel.tsx +++ b/frontend/src/renderer/src/components/SidePanel.tsx @@ -80,7 +80,7 @@ export default function SidePanel({ return ( <div className="w-80 flex flex-col gap-2 p-4 pr-0 pb-0"> - <div className="bg-blue-200 border border-blue-500 p-3 rounded-lg"> + <div className="bg-blue-200 border border-blue-500 p-2 px-4 rounded-lg"> <h2 className="font-bold text-blue-500">STATUS</h2> </div> <div className="flex-1 p-3 space-y-2 bg-blue-200 border border-blue-500 rounded-lg"> diff --git a/frontend/src/renderer/src/components/TitleBar.tsx b/frontend/src/renderer/src/components/TitleBar.tsx index 3e840bc6a505cc66b2875c59f15593e22e191503..4aa26b5574e907deb0e141d3ab967a626b429034 100644 --- a/frontend/src/renderer/src/components/TitleBar.tsx +++ b/frontend/src/renderer/src/components/TitleBar.tsx @@ -6,7 +6,7 @@ const handleClose = (): void => window.electron.ipcRenderer.send('close-window') export default function TitleBar(): JSX.Element { return ( - <div className="titlebar-drag bg-orange-400 text-white flex justify-between items-center px-4 py-2 rounded-t-2xl"> + <div className="titlebar-drag bg-blue-500 text-white flex justify-between items-center px-4 py-2 rounded-t-2xl"> <div className="font-bold text-lg">Car Controller</div> <div className="flex gap-2 text-xl titlebar-no-drag"> <button onClick={handleMinimize} className="hover:bg-yellow-400 p-1 rounded"> diff --git a/ipcExample b/ipcExample deleted file mode 100644 index 974d32c6bf92b58fbf7c1407fa46e59949617f8e..0000000000000000000000000000000000000000 --- a/ipcExample +++ /dev/null @@ -1,20 +0,0 @@ -import CarController from './components/CarController' -import TitleBar from './components/TitleBar' - -function App(): JSX.Element { - // const ipcHandle = (): void => window.electron.ipcRenderer.send('ping') - - return ( - // <div className="action"> - // <a target="_blank" rel="noreferrer" onClick={ipcHandle}> - // Send IPC - // </a> - // </div> - <div className="w-full h-full bg-blue-500 flex flex-col"> - <TitleBar /> - <CarController /> - </div> - ) -} - -export default App diff --git a/kill_servers.sh b/kill_servers.sh new file mode 100755 index 0000000000000000000000000000000000000000..968225ca603207f6ef2d1fbdb063d77d5ef37e35 --- /dev/null +++ b/kill_servers.sh @@ -0,0 +1,23 @@ +#!/bin/bash + +echo "Stopping Bluetooth and Web servers..." + +# Kill Bluetooth Server +pkill -f "python3 /home/pi/TurboPi/lab2/bluetooth/bt_server.py" +if [ $? -eq 0 ]; then + echo "Bluetooth server stopped." +else + echo "No Bluetooth server running." +fi + +# Kill Web Server +pkill -f "python3 /home/pi/TurboPi/lab2/backend/server.py" +if [ $? -eq 0 ]; then + echo "Web server stopped." +else + echo "No Web server running." +fi + +# Confirm processes are stopped +sleep 1 +ps aux | grep "python3" | grep -v "grep" diff --git a/start_servers.sh b/start_servers.sh new file mode 100755 index 0000000000000000000000000000000000000000..057fc6811d189c742a48c810d334eebca7c0fb8c --- /dev/null +++ b/start_servers.sh @@ -0,0 +1,14 @@ +#!/bin/bash + +# Start Bluetooth Server +echo "Starting Bluetooth server..." +nohup /bin/python3 /home/pi/TurboPi/lab2/bluetooth/bt_server.py > /home/pi/TurboPi/lab2/bluetooth/bt_server.log 2>&1 & + +# Start Web Server +echo "Starting Web server..." +nohup /bin/python3 /home/pi/TurboPi/lab2/backend/server.py > /home/pi/TurboPi/lab2/backend/server.log 2>&1 & + +# Display running processes +echo "Servers are starting..." +sleep 2 +ps aux | grep "python3" | grep -v "grep"