From 054dafbab5b72e4dd01af2cedee81a6ece5bd94c Mon Sep 17 00:00:00 2001 From: quietCoder500 Date: Mon, 17 Jul 2023 20:20:28 -0400 Subject: [PATCH 1/4] Update to Python 3 --- Example/wiimote.py | 37 ++++++++++++++++++++----------------- 1 file changed, 20 insertions(+), 17 deletions(-) diff --git a/Example/wiimote.py b/Example/wiimote.py index 507c384..7dbf7a5 100644 --- a/Example/wiimote.py +++ b/Example/wiimote.py @@ -6,24 +6,27 @@ import cwiid, time -button_delay = 0.1 +button_delay = 0.5 -print 'Please press buttons 1 + 2 on your Wiimote now ...' +print('Please press buttons 1 + 2 on your Wiimote now ...') time.sleep(1) # This code attempts to connect to your Wiimote and if it fails the program quits try: wii=cwiid.Wiimote() except RuntimeError: - print "Cannot connect to your Wiimote. Run again and make sure you are holding buttons 1 + 2!" + print("Cannot connect to your Wiimote. Run again and make sure you are holding buttons 1 + 2!") quit() -print 'Wiimote connection established!\n' -print 'Go ahead and press some buttons\n' -print 'Press PLUS and MINUS together to disconnect and quit.\n' +print('Wiimote connection established!\n') +print('Go ahead and press some buttons\n') +print('Press PLUS and MINUS together to disconnect and quit.\n') time.sleep(3) +for i in range(16): + wii.led = i + time.sleep(0.5) wii.rpt_mode = cwiid.RPT_BTN while True: @@ -32,7 +35,7 @@ # Detects whether + and - are held down and if they are it quits the program if (buttons - cwiid.BTN_PLUS - cwiid.BTN_MINUS == 0): - print '\nClosing connection ...' + print('\nClosing connection ...') # NOTE: This is how you RUMBLE the Wiimote wii.rumble = 1 time.sleep(1) @@ -41,35 +44,35 @@ # The following code detects whether any of the Wiimotes buttons have been pressed and then prints a statement to the screen! if (buttons & cwiid.BTN_LEFT): - print 'Left pressed' + print('Left pressed') time.sleep(button_delay) if(buttons & cwiid.BTN_RIGHT): - print 'Right pressed' + print('Right pressed') time.sleep(button_delay) if (buttons & cwiid.BTN_UP): - print 'Up pressed' + print('Up pressed') time.sleep(button_delay) if (buttons & cwiid.BTN_DOWN): - print 'Down pressed' + print('Down pressed') time.sleep(button_delay) if (buttons & cwiid.BTN_1): - print 'Button 1 pressed' + print('Button 1 pressed') time.sleep(button_delay) if (buttons & cwiid.BTN_2): - print 'Button 2 pressed' + print('Button 2 pressed') time.sleep(button_delay) if (buttons & cwiid.BTN_A): - print 'Button A pressed' + print('Button A pressed') time.sleep(button_delay) if (buttons & cwiid.BTN_B): - print 'Button B pressed' + print('Button B pressed') time.sleep(button_delay) if (buttons & cwiid.BTN_HOME): @@ -82,9 +85,9 @@ time.sleep(button_delay) if (buttons & cwiid.BTN_MINUS): - print 'Minus Button pressed' + print('Minus Button pressed') time.sleep(button_delay) if (buttons & cwiid.BTN_PLUS): - print 'Plus Button pressed' + print('Plus Button pressed') time.sleep(button_delay) From 09a9a3a00cb1d6b4a9b38fb32fecd12567aa65b2 Mon Sep 17 00:00:00 2001 From: quietCoder500 Date: Sun, 17 Dec 2023 15:00:56 -0500 Subject: [PATCH 2/4] Rc Car --- .vscode/settings.json | 14 +++ Example/balanceGraph.py | 168 ++++++++++++++++++++++++++++++ Example/fly_readings.py | 63 ++++++++++++ Example/weightdemo.py | 62 +++++++++++ Example/wiiKalman.py | 154 ++++++++++++++++++++++++++++ Example/wiimote.py | 5 +- README.md | 22 ---- RobotRemote.ino | 142 ++++++++++++++++++++++++++ bardCenter.py | 44 ++++++++ boardEXTtest.py | 12 +++ carDriver.py | 221 ++++++++++++++++++++++++++++++++++++++++ rcCarWifiRemote.py | 111 ++++++++++++++++++++ setup.sh | 3 - 13 files changed, 994 insertions(+), 27 deletions(-) create mode 100644 .vscode/settings.json create mode 100644 Example/balanceGraph.py create mode 100644 Example/fly_readings.py create mode 100644 Example/weightdemo.py create mode 100644 Example/wiiKalman.py delete mode 100644 README.md create mode 100644 RobotRemote.ino create mode 100644 bardCenter.py create mode 100644 boardEXTtest.py create mode 100644 carDriver.py create mode 100644 rcCarWifiRemote.py delete mode 100644 setup.sh diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..f731b4b --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,14 @@ +{ + "cSpell.words": [ + "calcweight", + "comx", + "comy", + "fivethirtyeight", + "KEYDOWN", + "KEYUP", + "libardrone", + "navdata", + "NUNCHUK", + "Wiimote" + ] +} \ No newline at end of file diff --git a/Example/balanceGraph.py b/Example/balanceGraph.py new file mode 100644 index 0000000..2388814 --- /dev/null +++ b/Example/balanceGraph.py @@ -0,0 +1,168 @@ +import matplotlib.pyplot as plt +import matplotlib.animation as animation +from matplotlib import style +import numpy as np +import cwiid, time # noqa: E401 + +button_delay = 0.5 +TOP_RIGHT = 0 +BOTTOM_RIGHT = 1 +TOP_LEFT = 2 +BOTTOM_LEFT = 3 + +style.use('fivethirtyeight') + +fig = plt.figure() +ax1 = fig.add_subplot(1,1,1) + +print('Please press buttons 1 + 2 on your Wiimote now ...') +time.sleep(1) + +# This code attempts to connect to your Wiimote and if it fails the program quits +try: + wii=cwiid.Wiimote() +except RuntimeError: + print("Cannot connect to your Wiimote. Run again and make sure you are holding buttons 1 + 2!") + quit() +wii.rpt_mode = cwiid.RPT_BALANCE +""" +b2i = lambda b: int(b.encode("hex"), 16) # noqa: E731 + +calibration = wii.get_balance_cal() + +def get_mass(data): + return { + 'top_right': calc_mass(data["right_top"], TOP_RIGHT), + 'bottom_right': calc_mass(data["right_bottom"], BOTTOM_RIGHT), + 'top_left': calc_mass(data["left_top"], TOP_LEFT), + 'bottom_left': calc_mass(data["left_bottom"], BOTTOM_LEFT), + } + +def calc_mass(raw, pos): + # Calculates the Kilogram weight reading from raw data at position pos + # calibration[0] is calibration values for 0kg + # calibration[1] is calibration values for 17kg + # calibration[2] is calibration values for 34kg + if raw < calibration[pos][0]: + return 0.0 + elif raw < calibration[pos][1]: + return 17 * ((raw - calibration[pos][0]) / + float((calibration[pos][1] - + calibration[pos][0]))) + else: # if raw >= calibration[pos][1]: + return 17 + 17 * ((raw - calibration[pos][1]) / + float((calibration[pos][2] - + calibration[pos][1]))) + +def animate(i): + mass = get_mass(wii.state["balance"]) + comx = 1.0 + comy = 1.0 + try: + total_right = mass['top_right'] + mass['bottom_right'] + total_left = mass['top_left'] + mass['bottom_left'] + comx = total_right / total_left + if comx > 10: + comx = 10 - total_right / total_left + else: + comx -= 10 + total_bottom = mass['bottom_left'] + mass['bottom_right'] + total_top = mass['top_left'] + mass['top_right'] + comy = total_bottom / total_top + if comy > 10: + comy = 10 - total_top / total_bottom + else: + comy -= 10 + except BaseException: + pass + comx = round(comx, 2) + comy = round(comy, 2) + print("Center of mass: %s"%str({'x': comx, 'y': comy})) + # plot(x,y) using pygame or any other GUI + #ax1.clear() + ax1.plot(comx, comy, "bo") +""" + +balance_calibration = wii.get_balance_cal() +named_calibration = { + 'right_top': balance_calibration[0], + 'right_bottom': balance_calibration[1], + 'left_top': balance_calibration[2], + 'left_bottom': balance_calibration[3], +} + + +def get_vals(): + weights = [] + readings = wii.state["balance"] + for sensor in ('right_top', 'right_bottom', 'left_top', 'left_bottom'): + reading = readings[sensor] + calibration = named_calibration[sensor] + + if reading > calibration[2]*2: + print("Warning", sensor, "reading above upper calibration value") + # 1700 appears to be the step the calibrations are against. + # 17kg per sensor is 68kg, 1/2 of the advertised Japanese weight limit. + if reading < calibration[1]: + weights.append(1700 * (reading - calibration[0]) / (calibration[1] - calibration[0])) + else: + weights.append(1700 * (reading - calibration[1]) / (calibration[2] - calibration[1]) + 1700) + return weights + +def calcweight( readings, calibrations ): + """ + Determine the weight of the user on the board in hundredths of a kilogram + """ + weight = 0 + for sensor in ('right_top', 'right_bottom', 'left_top', 'left_bottom'): + reading = readings[sensor] + calibration = calibrations[sensor] + + # 1700 appears to be the step the calibrations are against. + # 17kg per sensor is 68kg, 1/2 of the advertised Japanese weight limit. + if reading < calibration[1]: + weight += 1700 * (reading - calibration[0]) / (calibration[1] - calibration[0]) + else: + weight += 1700 * (reading - calibration[1]) / (calibration[2] - calibration[1]) + 1700 + + return weight + +def calculate_centroid(sensor_readings, sensor_positions): + # Normalize sensor readings + normalized_readings = sensor_readings / np.sum(sensor_readings) + + # Calculate relative positions + relative_positions = np.dot(normalized_readings, sensor_positions) + + return relative_positions + +def convert_relative_positions_to_coordinates(relative_positions, scaling_factor): + # Convert relative positions to coordinates + coordinates = relative_positions * scaling_factor + + if round(((calcweight(wii.state['balance'], named_calibration) / 100.0)*2.205)-add, 1) > 35.0: + return coordinates + else: + ax1.clear() + return [0,0] + +# Example usage +sensor_positions = np.array([[68, 68], [68, -68], [-68, 68], [-68, -68]]) +scaling_factor = 1 + +time.sleep(3) +print("Type anything to balance weight") +c = input() +add = (calcweight(wii.state['balance'], named_calibration) / 100.0)*2.205 +wii.request_status() +print("Step On!") +time.sleep(3) + +def animate(i) -> None: + centroid = calculate_centroid(get_vals(), sensor_positions) + coordinates = convert_relative_positions_to_coordinates(centroid, scaling_factor) + + ax1.plot(coordinates[0], coordinates[1], "go") + +ani = animation.FuncAnimation(fig, animate, interval=10) +plt.show() \ No newline at end of file diff --git a/Example/fly_readings.py b/Example/fly_readings.py new file mode 100644 index 0000000..ab539fc --- /dev/null +++ b/Example/fly_readings.py @@ -0,0 +1,63 @@ +import cwiid, time + +button_delay = 0.5 + +print('Please press buttons 1 + 2 on your Wiimote now ...') +time.sleep(1) + +# This code attempts to connect to your Wiimote and if it fails the program quits +try: + Wii=cwiid.Wiimote() +except RuntimeError: + print("Cannot connect to your Wiimote. Run again and make sure you are holding buttons 1 + 2!") + quit() + +print('Wiimote connection established!\n') +print('Go ahead and press some buttons\n') +print('Press PLUS and MINUS together to disconnect and quit.\n') + +time.sleep(3) +Wii.rpt_mode = cwiid.RPT_BTN | cwiid.RPT_ACC | cwiid.RPT_EXT +Wii.led = 5 + #Here we handle the nunchuk, along with the joystick and the buttons +while(1): + time.sleep(0.2) + if Wii.state.get('nunchuk'): + try: + #Here is the data for the nunchuk stick: + #X axis:LeftMax = 25, Middle = 125, RightMax = 225 + NunchukStickX = (Wii.state['nunchuk']['stick'][cwiid.X]) + #Y axis:DownMax = 30, Middle = 125, UpMax = 225 + NunchukStickY = (Wii.state['nunchuk']['stick'][cwiid.Y]) + #The 'NunchukStickX' and the 'NunchukStickY' variables now store the stick values + + #Make it so that we can control the arm with the joystick + if (NunchukStickX < 60): + print("left") + if (NunchukStickX > 190): + print("right") + if (NunchukStickY < 60): + print("down") + if (NunchukStickY > 190): + print("up") + + #Here we create a variable to store the nunchuck button data + #0 = no buttons pressed + #1 = Z is pressed + #2 = C is pressed + #3 = Both C and Z are pressed + ChukBtn = Wii.state['nunchuk']['buttons'] + if (ChukBtn == 1): + print("Z btn") + if (ChukBtn == 2): + print("C btn") + #If both are pressed the led blinks + if (ChukBtn == 3): + print("both btn") + + + except: + pass + else: + print(Wii.state) + time.sleep(1) \ No newline at end of file diff --git a/Example/weightdemo.py b/Example/weightdemo.py new file mode 100644 index 0000000..817116c --- /dev/null +++ b/Example/weightdemo.py @@ -0,0 +1,62 @@ +import sys +from time import sleep +import cwiid + +def main(): + #Connect to address given on command-line, if present + print('Put Wiimote in discoverable mode now (press 1+2)...') + global wiimote + if len(sys.argv) > 1: + wiimote = cwiid.Wiimote(sys.argv[1]) + else: + wiimote = cwiid.Wiimote() + + wiimote.rpt_mode = cwiid.RPT_BALANCE + wiimote.request_status() + balance_calibration = wiimote.get_balance_cal() + named_calibration = { 'right_top': balance_calibration[0], + 'right_bottom': balance_calibration[1], + 'left_top': balance_calibration[2], + 'left_bottom': balance_calibration[3], + } + + sleep(3) + print("Type anything to balance weight") + c = sys.stdin.read(1) + add = (calcweight(wiimote.state['balance'], named_calibration) / 100.0)*2.205 + wiimote.request_status() + print("Step On!") + sleep(3) + + while True: + print("Type q to quit, or anything else to report your weight") + c = input() + if c == 'q': + break + wiimote.request_status() + print(round(((calcweight(wiimote.state['balance'], named_calibration) / 100.0)*2.205)-add, 1), "Lbs") + + return 0 + +def calcweight( readings, calibrations ): + """ + Determine the weight of the user on the board in hundredths of a kilogram + """ + weight = 0 + for sensor in ('right_top', 'right_bottom', 'left_top', 'left_bottom'): + reading = readings[sensor] + calibration = calibrations[sensor] + + if reading > calibration[2]: + print("Warning", sensor, "reading above upper calibration value") + # 1700 appears to be the step the calibrations are against. + # 17kg per sensor is 68kg, 1/2 of the advertised Japanese weight limit. + if reading < calibration[1]: + weight += 1700 * (reading - calibration[0]) / (calibration[1] - calibration[0]) + else: + weight += 1700 * (reading - calibration[1]) / (calibration[2] - calibration[1]) + 1700 + + return weight + +if __name__ == "__main__": + sys.exit(main()) \ No newline at end of file diff --git a/Example/wiiKalman.py b/Example/wiiKalman.py new file mode 100644 index 0000000..7813da2 --- /dev/null +++ b/Example/wiiKalman.py @@ -0,0 +1,154 @@ +import cwiid +from time import time, asctime, sleep, perf_counter +from numpy import * +from pylab import * +import math +import numpy as np +from operator import add +HPF = 0.98 +LPF = 0.02 + +def calibrate(wiimote): + print("Keep the remote still") + sleep(3) + print("Calibrating") + + messages = wiimote.get_mesg() + i=0 + accel_init = [] + angle_init = [] + while (i<1000): + sleep(0.01) + messages = wiimote.get_mesg() + for mesg in messages: + # Motion plus: + if mesg[0] == cwiid.MESG_MOTIONPLUS: + if record: + angle_init.append(mesg[1]['angle_rate']) + # Accelerometer: + elif mesg[0] == cwiid.MESG_ACC: + if record: + accel_init.append(list(mesg[1])) + i+=1 + + accel_init_avg = list(np.mean(np.array(accel_init), axis=0)) + print(accel_init_avg) + angle_init_avg = sum(angle_init)/len(angle_init) + print("Finished Calibrating") + return (accel_init_avg, angle_init_avg) + +def plotter(plot_title, timevector, data, position, n_graphs): + subplot(n_graphs, 1, position) + plot(timevector, data[0], "r", + timevector, data[1], "g", + timevector, data[2], "b") + xlabel("time (s)") + ylabel(plot_title) + +print("Press 1+2 on the Wiimote now") +wiimote = cwiid.Wiimote() + +# Rumble to indicate a connection +wiimote.rumble = 1 +print("Connection established - release buttons") +sleep(0.2) +wiimote.rumble = 0 +sleep(1.0) + +wiimote.enable(cwiid.FLAG_MESG_IFC | cwiid.FLAG_MOTIONPLUS) +wiimote.rpt_mode = cwiid.RPT_BTN | cwiid.RPT_ACC | cwiid.RPT_MOTIONPLUS + +accel_init, angle_init = calibrate(wiimote) +str = "" +print("Press plus to start recording, minus to end recording") +loop = True +record = False +accel_data = [] +angle_data = [] +messages = wiimote.get_mesg() +while (loop): + sleep(0.01) + messages = wiimote.get_mesg() + for mesg in messages: + # Motion plus: + if mesg[0] == cwiid.MESG_MOTIONPLUS: + if record: + angle_data.append({"Time" : perf_counter(), \ + "Rate" : mesg[1]['angle_rate']}) + # Accelerometer: + elif mesg[0] == cwiid.MESG_ACC: + if record: + accel_data.append({"Time" : perf_counter(), "Acc" : [mesg[1][i] - accel_init[i] for i in range(len(accel_init))]}) + # Button: + elif mesg[0] == cwiid.MESG_BTN: + if mesg[1] & cwiid.BTN_PLUS and not record: + print("Recording - press minus button to stop") + record = True + start_time = perf_counter() + if mesg[1] & cwiid.BTN_MINUS and record: + if len(accel_data) == 0: + print("No data recorded") + else: + print("End recording") + print("{0} data points in {1} seconds".format( + len(accel_data), perf_counter() - accel_data[0]["Time"])) + record = False + loop = False + else: + pass + +wiimote.disable(cwiid.FLAG_MESG_IFC | cwiid.FLAG_MOTIONPLUS) +if len(accel_data) == 0: + sys.exit() + + +timevector = [] +a = [[],[],[]] +v = [[],[],[]] +p = [[],[],[]] +last_time = 0 +velocity = [0,0,0] +position = [0,0,0] + +for n, x in enumerate(accel_data): + if (n == 0): + origin = x + else: + elapsed = x["Time"] - origin["Time"] + delta_t = x["Time"] - last_time + timevector.append(elapsed) + for i in range(3): + acceleration = x["Acc"][i] - origin["Acc"][i] + velocity[i] = velocity[i] + delta_t * acceleration + position[i] = position[i] + delta_t * velocity[i] + a[i].append(acceleration) + v[i].append(velocity[i]) + p[i].append(position[i]) + last_time = x["Time"] + +n_graphs = 3 +if len(angle_data) == len(accel_data): + n_graphs = 5 + angle_accel = [(math.pi)/2 if (j**2 + k**2)==0 else math.atan(i/math.sqrt(j**2 + k**2)) for i,j,k in zip(a[0],a[1],a[2])] + ar = [[],[],[]] # Angle rates + aa = [[],[],[]] # Angles + angle = [0,0,0] + for n, x in enumerate(angle_data): + if (n == 0): + origin = x + else: + delta_t = x["Time"] - last_time + for i in range(3): + rate = x["Rate"][i] - origin["Rate"][i] + angle[i] = HPF*(np.array(angle[i]) + delta_t * rate) + LPF*np.array(angle_accel) + ar[i].append(rate) + aa[i].append(angle[i]) + last_time = x["Time"] + + +plotter("Acceleration", timevector, a, 1, n_graphs) +if n_graphs == 5: + plotter("Angle Rate", timevector, ar, 4, n_graphs) + plotter("Angle", timevector, aa, 5, n_graphs) + +show() \ No newline at end of file diff --git a/Example/wiimote.py b/Example/wiimote.py index 7dbf7a5..67bf5e4 100644 --- a/Example/wiimote.py +++ b/Example/wiimote.py @@ -27,10 +27,10 @@ for i in range(16): wii.led = i time.sleep(0.5) -wii.rpt_mode = cwiid.RPT_BTN +wii.rpt_mode = cwiid.RPT_NUNCHUCK while True: - + buttons = wii.state['buttons'] # Detects whether + and - are held down and if they are it quits the program @@ -90,4 +90,5 @@ if (buttons & cwiid.BTN_PLUS): print('Plus Button pressed') + print(wii.state) time.sleep(button_delay) diff --git a/README.md b/README.md deleted file mode 100644 index a982e23..0000000 --- a/README.md +++ /dev/null @@ -1,22 +0,0 @@ -Wiimote -======= - -All the necessary code and commands for using a Wiimote with the Raspberry Pi (Python) - -This GitHub repository accompanies my YouTube tutorial for using the Nintendo Wiimote with the Raspberry Pi! You can find it here: https://www.youtube.com/watch?v=bO5-FjLe5xE&list=UUfY8sl5Q6VKndz0nLaGygPw - -I'll leave it to the tutorial to do the explaining! - -Enjoy! - -Matt - -The Raspberry Pi Guy - -www.youtube.com/theraspberrypiguy - -www.theraspberrypiguy.com - -theraspberrypiguy@gmail.com - -@RaspberryPiGuy1 diff --git a/RobotRemote.ino b/RobotRemote.ino new file mode 100644 index 0000000..a90fbbf --- /dev/null +++ b/RobotRemote.ino @@ -0,0 +1,142 @@ +#include +#include +#include "Freenove_4WD_Car_For_ESP32.h" +#include "Freenove_4WD_Car_WiFi.h" +#include +#include +#include + +#define OBSTACLE_DISTANCE 55 +#define OBSTACLE_DISTANCE_LOW 35 +int distance[4]; //Storage of ultrasonic data + +const int UDP_TX_PACKET_MAX_SIZE = 1024; +String CmdArray[8]; +int paramters[8]; +bool videoFlag = 0; +bool moving = false; +bool scanEnabled = true; + +unsigned int localPort = 4000; // local port to listen on + +// buffers for receiving and sending data +char packetBuffer[UDP_TX_PACKET_MAX_SIZE + 1]; // buffer to hold incoming packet, +char ReplyBuffer[] = "acknowledged\r\n"; // a string to send back + +WiFiUDP Udp; + +void WiFi_Init() { + ssid_Router = "Stoltzfus-2G"; //Modify according to your router name + password_Router = "Farhills"; //Modify according to your router password + ssid_AP = "Sunshine"; //ESP32 turns on an AP and calls it Sunshine + password_AP = "Sunshine"; //Set your AP password for ESP32 to Sunshine + frame_size = FRAMESIZE_CIF;//400*296 +} + +WiFiServer server_Cmd(4000); + + + +void setup() { + Serial.begin(115200); + Serial.setDebugOutput(true); + Buzzer_Setup(); //Buzzer initialization + + WiFi_Init(); //WiFi paramters initialization + WiFi_Setup(0); //Start AP Mode + server_Cmd.begin(4000); //Start the command server + + Ultrasonic_Setup(); + PCA9685_Setup(); //PCA9685 initialization + Light_Setup(); //Light initialization + Track_Setup(); //Track initialization + + disableCore0WDT(); //Turn off the watchdog function in kernel 0 + xTaskCreateUniversal(loopTask_WTD, "loopTask_WTD", 8192, NULL, 0, NULL, 0); + Servo_2_Angle(105); + Udp.begin(localPort); +} + +void Get_Command(String inputStringTemp) +{ + int string_length = inputStringTemp.length(); + for (int i = 0; i < 8; i++) {//Parse the command received by WiFi + int index = inputStringTemp.indexOf(INTERVAL_CHAR); + if (index < 0) { + if (string_length > 0) { + CmdArray[i] = inputStringTemp; //Get command + paramters[i] = inputStringTemp.toInt();//Get parameters + } + break; + } + else { + string_length -= index; //Count the remaining words + CmdArray[i] = inputStringTemp.substring(0, index); //Get command + paramters[i] = CmdArray[i].toInt(); //Get parameters + inputStringTemp = inputStringTemp.substring(index + 1);//Update string + } + } +} +void loop() { + // if there's data available, read a packet + int packetSize = Udp.parsePacket(); + if (packetSize) { + // read the packet into packetBufffer + int n = Udp.read(packetBuffer, UDP_TX_PACKET_MAX_SIZE); + packetBuffer[n] = 0; + Serial.println(packetBuffer);//Print out the command received by WiFi + String inputStringTemp = packetBuffer; + Get_Command(inputStringTemp); + if (CmdArray[0] == CMD_BUZZER) //Buzzer control command + { + Buzzer_Variable(paramters[1], paramters[2]);} + if (CmdArray[0] == CMD_MOTOR) {//Network control car movement command + Car_SetMode(0); + if (paramters[1] == 0 && paramters[3] == 0){ + Motor_Move(0, 0, 0, 0);//Stop the car + moving = false;} + else {//If the parameters are not equal to 0 + Motor_Move(paramters[1], paramters[1], paramters[3], paramters[3]); + if (paramters[1] >= 150 && paramters[3] >= 150) + moving = true; + } + } + if (CmdArray[0] == CMD_SERVO) {//Network control servo motor movement command + if (paramters[1] == 0) + Servo_1_Angle(paramters[2]); + else if (paramters[1] == 1) + Servo_2_Angle(paramters[2]); + } + if (CmdArray[0] == CMD_USONIC) { + if (scanEnabled) { + scanEnabled = false; + char replyBuff = 'd'; + Udp.beginPacket(Udp.remoteIP(), Udp.remotePort()); + Udp.write(replyBuff); + Udp.endPacket(); + } + else { + scanEnabled = true; + char replyBuff = 'e'; + Udp.beginPacket(Udp.remoteIP(), Udp.remotePort()); + Udp.write(replyBuff); + Udp.endPacket(); + } + } + if (CmdArray[0] == CMD_CAR_MODE) { //Car command Mode + Car_SetMode(paramters[1]); + } + //Clears the command array and parameter array + memset(CmdArray, 0, sizeof(CmdArray)); + memset(paramters, 0, sizeof(paramters)); + } + else { + if (moving && scanEnabled) { + if (Get_Sonar() < OBSTACLE_DISTANCE_LOW) { + Motor_Move(0, 0, 0, 0); + moving = false; + } + } + } + Car_Select(carFlag);//ESP32 Car mode selection function + } diff --git a/bardCenter.py b/bardCenter.py new file mode 100644 index 0000000..1d00a4e --- /dev/null +++ b/bardCenter.py @@ -0,0 +1,44 @@ +""" + +Converting the pressure sensor readings from four corners to x, y coordinates involves using a technique called centroid calculation. The centroid is the geometric center of a planar shape, and it can be calculated by averaging the x-coordinates and y-coordinates of the points that define the shape. + +In this case, the four pressure sensors define a rectangular shape with the centroid at the center. The pressure sensor readings can be used to calculate the relative positions of the sensors with respect to the centroid. Then, the relative positions can be converted to x, y coordinates by using a scaling factor. + +Here is a step-by-step explanation of the math involved: + +Normalize sensor readings: Normalize the sensor readings by dividing each reading by the sum of all readings. This ensures that the normalized readings represent the relative contribution of each sensor to the overall force. + +Calculate relative positions: Calculate the relative positions of the sensors with respect to the centroid. This can be done by multiplying the normalized readings by the distance between the corresponding sensor and the centroid. + +Convert relative positions to x, y coordinates: Convert the relative positions to x, y coordinates by using a scaling factor. The scaling factor represents the conversion between the units of the relative positions (e.g., centimeters) and the units of the desired x, y coordinates (e.g., meters). + +Here is an example of how to implement this process in Python: +""" + +import numpy as np + +def calculate_centroid(sensor_readings, sensor_positions): + # Normalize sensor readings + normalized_readings = sensor_readings / np.sum(sensor_readings) + + # Calculate relative positions + relative_positions = np.dot(normalized_readings, sensor_positions) + + return relative_positions + +def convert_relative_positions_to_coordinates(relative_positions, scaling_factor): + # Convert relative positions to coordinates + coordinates = relative_positions * scaling_factor + + return coordinates + +# Example usage +sensor_readings = [10, 20, 30, 40] +sensor_positions = np.array([[1, 1], [-1, 1], [-1, -1], [1, -1]]) +scaling_factor = 0.1 + +centroid = calculate_centroid(sensor_readings, sensor_positions) +coordinates = convert_relative_positions_to_coordinates(centroid, scaling_factor) + +print("Centroid:", centroid) +print("Coordinates:", coordinates) \ No newline at end of file diff --git a/boardEXTtest.py b/boardEXTtest.py new file mode 100644 index 0000000..361cb23 --- /dev/null +++ b/boardEXTtest.py @@ -0,0 +1,12 @@ +import time +import cwiid +wii=cwiid.Wiimote() + +start = time.time() +while True: + val = wii.state + if val["ext_type"] == 3: + break +end_time = time.time() + +print("total time taken:", end_time - start) \ No newline at end of file diff --git a/carDriver.py b/carDriver.py new file mode 100644 index 0000000..c4efef5 --- /dev/null +++ b/carDriver.py @@ -0,0 +1,221 @@ +import numpy as np +import cwiid, time # noqa: E401 +import socket + +print('Please press SYNC on your Wii board now ...') +time.sleep(1) + +wiiBoard = None +wiiMote = None +# This code attempts to connect to your Wiimote and if it fails the program quits +while wiiBoard is None or wiiMote is None: + try: + wii=cwiid.Wiimote() + wii.rpt_mode = cwiid.RPT_BALANCE + time.sleep(0.1) + if wii.state["ext_type"] == cwiid.EXT_BALANCE: + wiiBoard = wii + wii = None + wiiBoard.rpt_mode = cwiid.RPT_BALANCE | cwiid.RPT_BTN + wiiBoard.led = 1 + print("Board Connected") + else: + wiiMote = wii + wii = None + wiiMote.rpt_mode = cwiid.RPT_BTN | cwiid.RPT_NUNCHUK + wiiMote.led = cwiid.LED1_ON | cwiid.LED4_ON + print("Wiimote connected") + except RuntimeError: + print("Cannot connect to your Wiimote or Wii Board. Run again and make sure you are holding buttons 1 + 2 (or SYNC)!") + + +try: + wiiMote.state["nunchuk"] +except KeyError: + print("Cannot connect to your Nunchuk. Make sure it is plugged in!") + quit() +balance_calibration = wiiBoard.get_balance_cal() +named_calibration = { + 'right_top': balance_calibration[0], + 'right_bottom': balance_calibration[1], + 'left_top': balance_calibration[2], + 'left_bottom': balance_calibration[3], +} +def connect_socket(): + connection = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + connection.settimeout(0.4) + return connection + + +def get_vals() -> list: + weights = [] + readings = wiiBoard.state["balance"] + for sensor in ('right_top', 'right_bottom', 'left_top', 'left_bottom'): + reading = readings[sensor] + calibration = named_calibration[sensor] + + if reading > calibration[2]*2: + print("Warning", sensor, "reading above upper calibration value") + # 1700 appears to be the step the calibrations are against. + # 17kg per sensor is 68kg, 1/2 of the advertised Japanese weight limit. + if reading < calibration[1]: + weights.append(1700 * (reading - calibration[0]) / (calibration[1] - calibration[0])) + else: + weights.append(1700 * (reading - calibration[1]) / (calibration[2] - calibration[1]) + 1700) + return weights + +def calcweight( readings, calibrations ) -> int: + """ + Determine the weight of the user on the board in hundredths of a kilogram + """ + weight = 0 + for sensor in ('right_top', 'right_bottom', 'left_top', 'left_bottom'): + reading = readings[sensor] + calibration = calibrations[sensor] + + # 1700 appears to be the step the calibrations are against. + # 17kg per sensor is 68kg, 1/2 of the advertised Japanese weight limit. + if reading < calibration[1]: + weight += 1700 * (reading - calibration[0]) / (calibration[1] - calibration[0]) + else: + weight += 1700 * (reading - calibration[1]) / (calibration[2] - calibration[1]) + 1700 + + return weight + +def calculate_centroid(sensor_readings, sensor_positions): + # Normalize sensor readings + normalized_readings = sensor_readings / np.sum(sensor_readings) + + # Calculate relative positions + relative_positions = np.dot(normalized_readings, sensor_positions) + + return relative_positions + +def convert_relative_positions_to_coordinates(relative_positions, scaling_factor) -> list[int]: + # Convert relative positions to coordinates + coordinates = relative_positions * scaling_factor + + if round(((calcweight(wiiBoard.state['balance'], named_calibration) / 100.0)*2.205)-add, 1) > 35.0: + return coordinates + else: + return [0,0] + +def getStandardSpeed(coordinate:int, dif=20) -> int: + if coordinate < 0: + speed = (coordinate+dif)*100 + if speed < -4000: + return 4000 + if speed > -2000: + return 2000 + return -speed + elif coordinate > 0: + speed = (coordinate-dif)*100 + if speed > 4000: + return 4000 + if speed < 2000: + return 2000 + return speed + else: + return 0 +# Example usage +sensor_positions = np.array([[68, 68], [68, -68], [-68, 68], [-68, -68]]) +scaling_factor = 1 + +connection = connect_socket() +""" +time.sleep(3) +print("Type anything to balance weight") +c = input() +add = (calcweight(wiiBoard.state['balance'], named_calibration) / 100.0)*2.205 +print("Step On!") +time.sleep(3)""" +add = 0 + +try: + while True: + # Raw knowledge + mote_state = wiiMote.state + board_state = wiiBoard.state + + # Temporary knowledge + centroid = calculate_centroid(get_vals(), sensor_positions) + + # Filtered knowledge + coordinates = convert_relative_positions_to_coordinates(centroid, scaling_factor) + weight = round(((calcweight(wiiBoard.state['balance'], named_calibration) / 100.0)*2.205)-add, 1) + + # Get direction + x = 0 + y = 0 + + if coordinates[0] >= 30: + x = 1 + elif coordinates[0] <= -30: + x = -1 + + if coordinates[1] >= 30: + y = 1 + elif coordinates[1] <= -30: + y = -1 + + moving = 0 + buttons = mote_state["buttons"] + + # Driving + if mote_state["buttons"] == cwiid.BTN_B: + connection.sendto(b"CMD_MOTOR#0#0#0#0", ("192.168.1.140", 4000)) + moving = 0 + time.sleep(0.2) + continue + + if 3 != moving and ((buttons-cwiid.BTN_A) == cwiid.BTN_LEFT): + connection.sendto(b"CMD_MOTOR#-500#-500#500#500", ("192.168.1.140", 4000)) + moving = 3 + + + elif 4 != moving and ((buttons-cwiid.BTN_A) == cwiid.BTN_RIGHT): + connection.sendto(b"CMD_MOTOR#500#500#-500#-500", ("192.168.1.140", 4000)) + moving = 4 + + elif 1 != moving and ((buttons-cwiid.BTN_A) == cwiid.BTN_UP): + connection.sendto(b"CMD_MOTOR#1500#1500#1500#1500", ("192.168.1.140", 4000)) + moving = 1 + + elif 2 != moving and ((buttons-cwiid.BTN_A) == cwiid.BTN_DOWN): + connection.sendto(b"CMD_MOTOR#-1500#-1500#-1500#-1500", ("192.168.1.140", 4000)) + moving = 2 + + elif buttons-cwiid.BTN_A == 0: + connection.sendto(b"CMD_MOTOR#0#0#0#0", ("192.168.1.140", 4000)) + moving = 0 + + + + + + + elif x == 0 and y == 0 and 0 != moving: + connection.sendto(b"CMD_MOTOR#0#0#0#0", ("192.168.1.140", 4000)) + moving = 0 + + elif 1 != moving and y == 1: + connection.sendto(b"CMD_MOTOR#1500#1500#1500#1500", ("192.168.1.140", 4000)) + moving = 1 + + elif 2 != moving and y == -1: + connection.sendto(b"CMD_MOTOR#-1500#-1500#-1500#-1500", ("192.168.1.140", 4000)) + moving = 2 + + elif 3 != moving and x == -1: + connection.sendto(b"CMD_MOTOR#-500#-500#500#500", ("192.168.1.140", 4000)) + moving = 3 + + elif 4 != moving and x == 1: + connection.sendto(b"CMD_MOTOR#500#500#-500#-500", ("192.168.1.140", 4000)) + moving = 4 + + time.sleep(0.1) +except KeyboardInterrupt: + wiiBoard.close() + wiiMote.close() + diff --git a/rcCarWifiRemote.py b/rcCarWifiRemote.py new file mode 100644 index 0000000..fa89ff2 --- /dev/null +++ b/rcCarWifiRemote.py @@ -0,0 +1,111 @@ +from imu import MPU6050 +import time +import network +import socket +from machine import Pin, I2C + +i2c = I2C(0, sda=Pin(0), scl=Pin(1), freq=400000) +imu = MPU6050(i2c) +btn_a = Pin(8, Pin.IN, Pin.PULL_UP) +btn_b = Pin(26, Pin.IN, Pin.PULL_UP) +btn_left = Pin(21, Pin.IN, Pin.PULL_UP) +btn_right = Pin(20, Pin.IN, Pin.PULL_UP) +btn_up = Pin(22, Pin.IN, Pin.PULL_UP) +btn_down = Pin(7, Pin.IN, Pin.PULL_UP) +red = Pin(15, Pin.OUT) +green = Pin(12, Pin.OUT) +blue = Pin(11, Pin.OUT) + +red.value(1) +green.value(0) +blue.value(1) + +ssid = 'Stoltzfus-2G' #Stoltzfus-2G +password = 'Farhills' #Farhills + +def connect(): + #Connect to WLAN + wlan = network.WLAN(network.STA_IF) + wlan.active(True) + wlan.connect(ssid, password) + while wlan.isconnected() is False: + print('Waiting for connection...') + time.sleep(1) + ip = wlan.ifconfig()[0] + print(f'Connected on {ip}') + return ip + +def connect_socket(): + connection = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + connection.settimeout(0.4) + return connection + +try: + ip = connect() + connection = connect_socket() +except KeyboardInterrupt: + machine.reset() + +moving = 0 +a_held = False +b_held = False +while True: + if not btn_b.value(): + if not b_held: + print("stop") + connection.sendto(b"CMD_MOTOR#0#0#0#0", ("192.168.1.140", 4000)) + moving = 0 + a_held = True + b_held = True + elif not btn_a.value(): + if not a_held: + connection.sendto(b"CMD_USONIC", ("192.168.1.140", 4000)) + try: + value = connection.recvfrom(1024)[0].decode() + print(value) + if value == 'e': + red.value(1) + green.value(0) + blue.value(1) + elif value == 'd': + red.value(0) + green.value(1) + blue.value(1) + except: + red.value(1) + green.value(1) + blue.value(0) + a_held = True + time.sleep(0.2) + elif b_held: + b_held = False + elif a_held: + a_held = False + if 1 != moving and not btn_up.value() : + print("start") + connection.sendto(b"CMD_MOTOR#2000#2000#2000#2000", ("192.168.1.140", 4000)) + moving = 1 + elif 2 != moving and not btn_down.value(): + print("rev") + connection.sendto(b"CMD_MOTOR#-1500#-1500#-1500#-1500", ("192.168.1.140", 4000)) + moving = 2 + elif 3 != moving and not btn_right.value(): + print("right") + connection.sendto(b"CMD_MOTOR#1000#1000#-1000#-1000", ("192.168.1.140", 4000)) + moving = 3 + elif 4 != moving and not btn_left.value(): + print("left") + connection.sendto(b"CMD_MOTOR#-1000#-1000#1000#1000", ("192.168.1.140", 4000)) + moving = 4 + elif not btn_up.value(): + pass + elif not btn_down.value(): + pass + elif not btn_right.value(): + pass + elif not btn_left.value(): + pass + elif moving > 0: + print("stop") + connection.sendto(b"CMD_MOTOR#0#0#0#0", ("192.168.1.140", 4000)) + moving = 0 \ No newline at end of file diff --git a/setup.sh b/setup.sh deleted file mode 100644 index 5bfc84a..0000000 --- a/setup.sh +++ /dev/null @@ -1,3 +0,0 @@ -sudo apt-get update -sudo apt-get install --no-install-recommends bluetooth -y -sudo apt-get install python-cwiid From 57c56da83208298ceb2363196215ef97abfd2eee Mon Sep 17 00:00:00 2001 From: quietCoder500 <132473475+quietCoder500@users.noreply.github.com> Date: Wed, 27 Dec 2023 20:59:30 +0000 Subject: [PATCH 3/4] classes --- Freenove-Car/ESP32Car.py | 13 +++++ Freenove-Car/WiiControls.py | 100 ++++++++++++++++++++++++++++++++++++ Freenove-Car/main.py | 43 ++++++++++++++++ RobotRemote.ino | 4 +- 4 files changed, 158 insertions(+), 2 deletions(-) create mode 100644 Freenove-Car/ESP32Car.py create mode 100644 Freenove-Car/WiiControls.py create mode 100644 Freenove-Car/main.py diff --git a/Freenove-Car/ESP32Car.py b/Freenove-Car/ESP32Car.py new file mode 100644 index 0000000..5487f09 --- /dev/null +++ b/Freenove-Car/ESP32Car.py @@ -0,0 +1,13 @@ +""" +For controlling Freenove's ESP32 Wrover 4WD Car +""" +import socket + +class Car: + """ + For connecting to and interacting + with Freenove's ESP32 Wrover 4WD Car + """ + + def __init__(self) -> None: + pass diff --git a/Freenove-Car/WiiControls.py b/Freenove-Car/WiiControls.py new file mode 100644 index 0000000..6f2e687 --- /dev/null +++ b/Freenove-Car/WiiControls.py @@ -0,0 +1,100 @@ +from abc import ABCMeta +import numpy as np +import cwiid, time + +def connect_multiple(devices: list) -> list: + """ + This connects multiple Wii devices in any order. + The device list must with the cwiid EXT_TYPE you want to connect to. + """ + print('Please press SYNC on your device now ...') + time.sleep(1) # wait for SYNC to be pressed + + found_devices = [] # to hold the classes with the wii device + + while len(found_devices) != len(devices): # loop until all are found + try: + wii=cwiid.Wiimote() # connect a remote + time.sleep(0.1) + if wii.state["ext_type"] == devices[len(found_devices)]: + if devices[len(found_devices)] == cwiid.EXT_BALANCE: + found_devices.append(Board(wii)) + else: + found_devices.append(Remote(wii)) + + except RuntimeError: + print("Cannot connect to your Wiimote or Wii Board. Run again and make sure you are holding buttons 1 + 2 (or SYNC)!") + return found_devices + + +class Board: + """ + For connecting to and interacting with a Wii Board + """ + + def __init__(self, cwiid_device) -> None: + self.device = cwiid_device + self.device.rpt_mode = cwiid.RPT_BALANCE | cwiid.RPT_BTN + self.device.led = 1 + self.ready = False + print("Board Connected") + + def calc_weight(self, readings) -> int: + """ + Determine the weight of the user on the board in hundredths of a kilogram + """ + weight = 0 + for sensor in ('right_top', 'right_bottom', 'left_top', 'left_bottom'): + reading = readings[sensor] + calibration = self.named_calibration[sensor] + + # 1700 appears to be the step the calibrations are against. + # 17kg per sensor is 68kg, 1/2 of the advertised Japanese weight limit. + if reading < calibration[1]: + weight += 1700 * (reading - calibration[0]) / (calibration[1] - calibration[0]) + else: + weight += 1700 * (reading - calibration[1]) / (calibration[2] - calibration[1]) + 1700 + + return weight + + def _calculate_centroid(self, sensor_readings, ): + sensor_positions = np.array([[68, 68], [68, -68], [-68, 68], [-68, -68]]) + # Normalize sensor readings + normalized_readings = sensor_readings / np.sum(sensor_readings) + + # Calculate relative positions + relative_positions = np.dot(normalized_readings, sensor_positions) + + return relative_positions + + def _convert_relative_positions_to_coordinates(self, relative_positions) -> list[int]: + # Convert relative positions to coordinates (pointless now) + coordinates = relative_positions + + if round(((self.calc_weight(self.device.state['balance'], self.named_calibration) / 100.0)*2.205)-self.weight_offset, 1) > 35.0: + return coordinates + else: + return [0,0] + + def prepare(self) -> None: + self.balance_calibration = self.device.get_balance_cal() + self.named_calibration = { + 'right_top': self.balance_calibration[0], + 'right_bottom': self.balance_calibration[1], + 'left_top': self.balance_calibration[2], + 'left_bottom': self.balance_calibration[3], + } + input("Step off!\n and enter something to balance weight: ") + self.weight_offset = (self.calc_weight(self.device.state['balance'], self.named_calibration) / 100.0)*2.205 + print("Step On!") + self.ready = True + + + +class Remote: + def __init__(self, cwiid_device) -> None: + self.device = cwiid_device + self.device.rpt_mode = cwiid.RPT_BTN + self.device.led = cwiid.LED1_ON | cwiid.LED4_ON + print("Wiimote Connected") + print("External type =", self.device.state["ext_type"]) diff --git a/Freenove-Car/main.py b/Freenove-Car/main.py new file mode 100644 index 0000000..b1a4576 --- /dev/null +++ b/Freenove-Car/main.py @@ -0,0 +1,43 @@ +import numpy as np +import cwiid, time # noqa: E401 + +print('Please press SYNC on your Wii board now ...') +time.sleep(1) + +wiiBoard = None +wiiMote = None +# This code attempts to connect to your Wiimote and if it fails the program quits +while wiiBoard is None or wiiMote is None: + try: + wii=cwiid.Wiimote() + wii.rpt_mode = cwiid.RPT_BALANCE + time.sleep(0.1) + if wii.state["ext_type"] == cwiid.EXT_BALANCE: + wiiBoard = wii + wii = None + wiiBoard.rpt_mode = cwiid.RPT_BALANCE | cwiid.RPT_BTN + wiiBoard.led = 1 + print("Board Connected") + else: + wiiMote = wii + wii = None + wiiMote.rpt_mode = cwiid.RPT_BTN | cwiid.RPT_NUNCHUK + wiiMote.led = cwiid.LED1_ON | cwiid.LED4_ON + print("Wiimote connected") + except RuntimeError: + print("Cannot connect to your Wiimote or Wii Board. Run again and make sure you are holding buttons 1 + 2 (or SYNC)!") + + +try: + wiiMote.state["nunchuk"] +except KeyError: + print("Cannot connect to your Nunchuk. Make sure it is plugged in!") + quit() +balance_calibration = wiiBoard.get_balance_cal() +named_calibration = { + 'right_top': balance_calibration[0], + 'right_bottom': balance_calibration[1], + 'left_top': balance_calibration[2], + 'left_bottom': balance_calibration[3], +} + diff --git a/RobotRemote.ino b/RobotRemote.ino index a90fbbf..00d1fbb 100644 --- a/RobotRemote.ino +++ b/RobotRemote.ino @@ -26,8 +26,8 @@ char ReplyBuffer[] = "acknowledged\r\n"; // a string to send back WiFiUDP Udp; void WiFi_Init() { - ssid_Router = "Stoltzfus-2G"; //Modify according to your router name - password_Router = "Farhills"; //Modify according to your router password + ssid_Router = ""; //Modify according to your router name + password_Router = ""; //Modify according to your router password ssid_AP = "Sunshine"; //ESP32 turns on an AP and calls it Sunshine password_AP = "Sunshine"; //Set your AP password for ESP32 to Sunshine frame_size = FRAMESIZE_CIF;//400*296 From 38db9537fac6962577186011ecb8b3ec10f484bd Mon Sep 17 00:00:00 2001 From: quietCoder500 Date: Mon, 15 Jan 2024 08:20:32 -0500 Subject: [PATCH 4/4] secrets --- rcCarWifiRemote.py | 3 --- 1 file changed, 3 deletions(-) diff --git a/rcCarWifiRemote.py b/rcCarWifiRemote.py index fa89ff2..6938ee6 100644 --- a/rcCarWifiRemote.py +++ b/rcCarWifiRemote.py @@ -20,9 +20,6 @@ green.value(0) blue.value(1) -ssid = 'Stoltzfus-2G' #Stoltzfus-2G -password = 'Farhills' #Farhills - def connect(): #Connect to WLAN wlan = network.WLAN(network.STA_IF)