From f5a56930231e1bb9a97756318861451ac6740422 Mon Sep 17 00:00:00 2001 From: FluffyCube9343 Date: Sat, 11 Mar 2023 14:37:26 -0500 Subject: [PATCH] Specify which degree for PID possible! --- Comms-Rewrite/bouncer/bouncer.ino | 4 + P_control_Code/P_control_Code.ino | 219 +++++++++++++------------- RaspPi-flask-recieve/server-nosend.py | 13 ++ RaspPi-flask-recieve/server.py | 2 +- 4 files changed, 130 insertions(+), 108 deletions(-) create mode 100644 RaspPi-flask-recieve/server-nosend.py diff --git a/Comms-Rewrite/bouncer/bouncer.ino b/Comms-Rewrite/bouncer/bouncer.ino index 4bf81a8..e5d4a92 100644 --- a/Comms-Rewrite/bouncer/bouncer.ino +++ b/Comms-Rewrite/bouncer/bouncer.ino @@ -15,6 +15,7 @@ void setup(){ } void loop(){ + if(Serial.available() > 0) { String data = Serial.readStringUntil('\n'); @@ -22,6 +23,9 @@ void loop(){ Serial.print("Hi Raspberry Pi! You sent me: "); Serial.println(String(i)); i+=1;} + else{ + Serial.println(data); + } } } diff --git a/P_control_Code/P_control_Code.ino b/P_control_Code/P_control_Code.ino index 3dcca52..c189719 100644 --- a/P_control_Code/P_control_Code.ino +++ b/P_control_Code/P_control_Code.ino @@ -28,7 +28,7 @@ // Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation // is used in I2Cdev.h #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE - #include "Wire.h" +#include "Wire.h" #endif // class default I2C address is 0x68 @@ -43,7 +43,7 @@ MPU6050 mpu(0x69); // <-- use for AD0 high depends on the MPU-6050's INT pin being connected to the Arduino's external interrupt #0 pin. On the Arduino Uno and Mega 2560, this is digital I/O pin 2. - * ========================================================================= */ + ========================================================================= */ /* ========================================================================= NOTE: Arduino v1.0.1 with the Leonardo board generates a compile error @@ -54,7 +54,7 @@ MPU6050 mpu(0x69); // <-- use for AD0 high http://arduino.cc/forum/index.php/topic,109987.0.html http://code.google.com/p/arduino/issues/detail?id=958 - * ========================================================================= */ + ========================================================================= */ @@ -75,7 +75,7 @@ double dererr = 0.0; double toc = millis(); Servo myservo; - + // uncomment "OUTPUT_READABLE_QUATERNION" if you want to see the actual @@ -137,7 +137,7 @@ float euler[3]; // [psi, theta, phi] Euler angle container float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector // packet structure for InvenSense teapot demo -uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' }; +uint8_t teapotPacket[14] = { '$', 0x02, 0, 0, 0, 0, 0, 0, 0, 0, 0x00, 0x00, '\r', '\n' }; @@ -147,7 +147,7 @@ uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\ volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high void dmpDataReady() { - mpuInterrupt = true; + mpuInterrupt = true; } @@ -158,88 +158,84 @@ void dmpDataReady() { void setup() { - -myservo.attach(3); - // join I2C bus (I2Cdev library doesn't do this automatically) - #if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE - Wire.begin(); - Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties - #elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE - Fastwire::setup(400, true); - #endif - // initialize serial communication - // (115200 chosen because it is required for Teapot Demo output, but it's - // really up to you depending on your project) - Serial.begin(115200); - while (!Serial); // wait for Leonardo enumeration, others continue immediately + myservo.attach(3); + // join I2C bus (I2Cdev library doesn't do this automatically) +#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE + Wire.begin(); + Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties +#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE + Fastwire::setup(400, true); +#endif - // NOTE: 8MHz or slower host processors, like the Teensy @ 3.3V or Arduino - // Pro Mini running at 3.3V, cannot handle this baud rate reliably due to - // the baud timing being too misaligned with processor ticks. You must use - // 38400 or slower in these cases, or use some kind of external separate - // crystal solution for the UART timer. + // initialize serial communication + // (115200 chosen because it is required for Teapot Demo output, but it's + // really up to you depending on your project) + Serial.begin(9600); + while (!Serial); // wait for Leonardo enumeration, others continue immediately - // initialize device - Serial.println(F("Initializing I2C devices...")); - mpu.initialize(); - pinMode(INTERRUPT_PIN, INPUT); + // NOTE: 8MHz or slower host processors, like the Teensy @ 3.3V or Arduino + // Pro Mini running at 3.3V, cannot handle this baud rate reliably due to + // the baud timing being too misaligned with processor ticks. You must use + // 38400 or slower in these cases, or use some kind of external separate + // crystal solution for the UART timer. - // verify connection - Serial.println(F("Testing device connections...")); - Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed")); + // initialize device + Serial.println(F("Initializing I2C devices...")); + mpu.initialize(); + pinMode(INTERRUPT_PIN, INPUT); - // wait for ready - Serial.println(F("\nSend any character to begin DMP programming and demo: ")); - while (Serial.available() && Serial.read()); // empty buffer - while (!Serial.available()); // wait for data - while (Serial.available() && Serial.read()); // empty buffer again + // verify connection + Serial.println(F("Testing device connections...")); + Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed")); - // load and configure the DMP - Serial.println(F("Initializing DMP...")); - devStatus = mpu.dmpInitialize(); + // wait for ready + Serial.println(F("\nSend any character to begin DMP programming and demo: ")); + // load and configure the DMP + Serial.println(F("Initializing DMP...")); + devStatus = mpu.dmpInitialize(); - // supply your own gyro offsets here, scaled for min sensitivity - mpu.setXGyroOffset(220); - mpu.setYGyroOffset(76); - mpu.setZGyroOffset(-85); - mpu.setZAccelOffset(1788); // 1688 factory default for my test chip + // supply your own gyro offsets here, scaled for min sensitivity + mpu.setXGyroOffset(220); + mpu.setYGyroOffset(76); + mpu.setZGyroOffset(-85); + mpu.setZAccelOffset(1788); // 1688 factory default for my test chip - // make sure it worked (returns 0 if so) - if (devStatus == 0) { - // Calibration Time: generate offsets and calibrate our MPU6050 - mpu.CalibrateAccel(6); - mpu.CalibrateGyro(6); - mpu.PrintActiveOffsets(); - // turn on the DMP, now that it's ready - Serial.println(F("Enabling DMP...")); - mpu.setDMPEnabled(true); + // make sure it worked (returns 0 if so) + if (devStatus == 0) { + // Calibration Time: generate offsets and calibrate our MPU6050 + mpu.CalibrateAccel(6); + mpu.CalibrateGyro(6); + mpu.PrintActiveOffsets(); + // turn on the DMP, now that it's ready + Serial.println(F("Enabling DMP...")); + mpu.setDMPEnabled(true); - // enable Arduino interrupt detection - Serial.print(F("Enabling interrupt detection (Arduino external interrupt ")); - Serial.print(digitalPinToInterrupt(INTERRUPT_PIN)); - Serial.println(F(")...")); - attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING); - mpuIntStatus = mpu.getIntStatus(); + // enable Arduino interrupt detection + Serial.print(F("Enabling interrupt detection (Arduino external interrupt ")); + Serial.print(digitalPinToInterrupt(INTERRUPT_PIN)); + Serial.println(F(")...")); + attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING); + mpuIntStatus = mpu.getIntStatus(); - // set our DMP Ready flag so the main loop() function knows it's okay to use it - Serial.println(F("DMP ready! Waiting for first interrupt...")); - dmpReady = true; + // set our DMP Ready flag so the main loop() function knows it's okay to use it + Serial.println(F("DMP ready! Waiting for first interrupt...")); + dmpReady = true; - // get expected DMP packet size for later comparison - packetSize = mpu.dmpGetFIFOPacketSize(); - } else { - // ERROR! - // 1 = initial memory load failed - // 2 = DMP configuration updates failed - // (if it's going to break, usually the code will be 1) - Serial.print(F("DMP Initialization failed (code ")); - Serial.print(devStatus); - Serial.println(F(")")); - } + // get expected DMP packet size for later comparison + packetSize = mpu.dmpGetFIFOPacketSize(); + } else { + // ERROR! + // 1 = initial memory load failed + // 2 = DMP configuration updates failed + // (if it's going to break, usually the code will be 1) + Serial.print(F("DMP Initialization failed (code ")); + Serial.print(devStatus); + Serial.println(F(")")); + } - // configure LED for outputj - pinMode(LED_PIN, OUTPUT); + // configure LED for outputj + pinMode(LED_PIN, OUTPUT); } @@ -249,41 +245,50 @@ myservo.attach(3); // ================================================================ void loop() { - tic = toc; - // if programming failed, don't try to do anything - if (!dmpReady) return; - // read a packet from FIFO - if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer)) { // Get the Latest packet + tic = toc; + if (Serial.available() > 0) { + String data = Serial.readStringUntil('\n'); + int data2 = data.toInt(); + goal = data2; + } + Serial.print(goal); + Serial.print("\t"); + // if programming failed, don't try to do anything + if (!dmpReady) return; + // read a packet from FIFO + if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer)) { // Get the Latest packet - #ifdef OUTPUT_READABLE_YAWPITCHROLL - // display Euler angles in degrees - mpu.dmpGetQuaternion(&q, fifoBuffer); - mpu.dmpGetGravity(&gravity, &q); - mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); - //Serial.print("ypr\t"); - Serial.print(ypr[0] * 180/M_PI); - Serial.print("\t"); - //Serial.print(ypr[1] * 180/M_PI); - //Serial.print("\t"); - //Serial.println(ypr[2] * 180/M_PI); +#ifdef OUTPUT_READABLE_YAWPITCHROLL + // display Euler angles in degrees + mpu.dmpGetQuaternion(&q, fifoBuffer); + mpu.dmpGetGravity(&gravity, &q); + mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); + //Serial.print("ypr\t"); + Serial.print(ypr[0] * 180 / M_PI); + Serial.print("\t"); + //Serial.print(ypr[1] * 180/M_PI); + //Serial.print("\t"); + //Serial.println(ypr[2] * 180/M_PI); - toc = millis(); - //dererr = (properr - (goal-ypr[0]))/(toc-tic); - properr = goal-ypr[0]; - //interr += properr * (toc-tic); - Serial.println(properr*kp); - int toRot = properr*kp; - int Rot = max(min(1500-toRot,2000),1000); - myservo.writeMicroseconds(Rot); + toc = millis(); + //dererr = (properr - (goal-ypr[0]))/(toc-tic); + properr = goal * M_PI / 180 - ypr[0]; + Serial.print(ypr[0]); + Serial.print("\t"); + //interr += properr * (toc-tic); + Serial.println(properr * kp); + int toRot = properr * kp; + int Rot = max(min(1500 - toRot, 2000), 1000); + myservo.writeMicroseconds(Rot); - - - #endif - // blink LED to indicate activity - blinkState = !blinkState; - digitalWrite(LED_PIN, blinkState); - } + +#endif + + // blink LED to indicate activity + blinkState = !blinkState; + digitalWrite(LED_PIN, blinkState); + } } diff --git a/RaspPi-flask-recieve/server-nosend.py b/RaspPi-flask-recieve/server-nosend.py new file mode 100644 index 0000000..1fdf902 --- /dev/null +++ b/RaspPi-flask-recieve/server-nosend.py @@ -0,0 +1,13 @@ +import socket +host='' +port=2345 +s=socket.socket() +s.bind((host,port)) +s.listen(10) +while True: + conn,addr=s.accept() + print("Connected by",addr) + data=conn.recv(1024) + print("received data:",data) + conn.send(data) + conn.close() diff --git a/RaspPi-flask-recieve/server.py b/RaspPi-flask-recieve/server.py index 045fd20..c38d32d 100644 --- a/RaspPi-flask-recieve/server.py +++ b/RaspPi-flask-recieve/server.py @@ -28,5 +28,5 @@ while True: receive_string = ser.readline().decode('utf-8').rstrip() print(receive_string) - conn.send(receive_string.encode()) + conn.send(data) conn.close()