Develop rpi

code/arduino/lighting_control/lighting_control.ino

@@ -0,0 +1,64 @@
+/*
+  Blink
+
+  Turns an LED on for one second, then off for one second, repeatedly.
+
+  Most Arduinos have an on-board LED you can control. On the UNO, MEGA and ZERO
+  it is attached to digital pin 13, on MKR1000 on pin 6. LED_BUILTIN is set to
+  the correct LED pin independent of which board is used.
+  If you want to know what pin the on-board LED is connected to on your Arduino
+  model, check the Technical Specs of your board at:
+  https://www.arduino.cc/en/Main/Products
+
+  modified 8 May 2014
+  by Scott Fitzgerald
+  modified 2 Sep 2016
+  by Arturo Guadalupi
+  modified 8 Sep 2016
+  by Colby Newman
+
+  This example code is in the public domain.
+
+  https://www.arduino.cc/en/Tutorial/BuiltInExamples/Blink
+*/
+int pin_11 = 11;
+int pin_12 = 12;
+// the setup function runs once when you press reset or power the board
+void setup() {
+  // initialize digital pin LED_BUILTIN as an output.
+  Serial.begin(9600);
+  pinMode(pin_11, OUTPUT);
+  pinMode(pin_12, OUTPUT);
+}
+int value = 0;
+// the loop function runs over and over again forever
+void loop() {
+  if (Serial.available() > 0) {
+    value = Serial.read();
+      if (value == 48) {
+         Serial.println("standby mode");
+         digitalWrite(pin_11, LOW);
+         digitalWrite(pin_12, LOW);
+      } else if (value == 49) {
+        Serial.println("go mode");
+        digitalWrite(pin_11, HIGH);
+        digitalWrite(pin_12, HIGH);
+      } else if (value == 50) {
+        Serial.println("party mode");
+        for(int i=0; i < 5; i++){ 
+          digitalWrite(pin_11, HIGH);
+          digitalWrite(pin_12, HIGH);
+          delay(600);
+          digitalWrite(pin_11, LOW);
+          digitalWrite(pin_12, LOW);
+          delay(600);
+          digitalWrite(pin_11, HIGH);
+          digitalWrite(pin_12, HIGH);
+          delay(340);
+          digitalWrite(pin_11, LOW);
+          digitalWrite(pin_12, LOW);
+          delay(400);
+        }
+      }
+   }
+}

code/rpi/src/flask_test_cpu.py

@@ -0,0 +1,14 @@
+from flask import Flask
+import subprocess
+
+app = Flask(__name__)
+
+@app.route("/")
+def hello_world():
+    return "<p>Hello, World!</p>"
+
+@app.route("/move")
+def run_script():
+    #subprocess.call("sudo pigpiod", shell=True)
+    subprocess.call("python3 /home/mrcoffee/move.py", shell=True)
+    return "<p>move arm</p>"

code/rpi/src/imu_data_plot.py

@@ -0,0 +1,135 @@
+"""IMU data playback with raw data plot and pose estimation 3d plot. (LSM9DS1 IMU)"""
+
+"""CSV file column format: 'temp', 'acc_x', 'acc_y', 'acc_z', 'gyro_x', 'gyro_y', 'gyro_z', 'mag_x', 'mag_y', 'mag_z', 'time'"""
+
+
+import pandas as pd
+import seaborn
+import matplotlib.pyplot as plt
+import sys
+import time
+import matplotlib.animation as animation
+import numpy as np
+
+class PoseEstimationIMU():
+    def __init__(self):
+        self.csv_file = ''
+        self.output = False
+        self.imu_data = None
+        self.input_data()
+        self.csv_to_pandas()
+        self.index = 0
+
+        # gyro calibration -> mean 25 observations 
+        self.acc_calibration = {'acc_x': 0, 'acc_y': 0, 'acc_z': 0}
+        self.gyro_calibration = {'gyro_x': 67.56,
+                                 'gyro_y': -131.56, 'gyro_z': 144.96}
+        self.mag_calibration = {'mag_x': 0, 'mag_y': 0, 'mag_z': 0}
+
+        self.data = pd.DataFrame(columns=['temp', 'acc_x', 'acc_y', 'acc_z',
+                                          'gyro_x', 'gyro_y', 'gyro_z', 'mag_x',
+                                          'mag_y', 'mag_z', 'time'])
+        # 2d projection
+        # self.fig, self.axs = plt.subplots(ncols=2, nrows=2, figsize=(7, 7))
+
+        # 3d projection
+        self.fig, self.axs = plt.subplots(
+            ncols=2, nrows=2, figsize=(7, 7), subplot_kw=dict(projection='3d'))
+        self.fig.suptitle(
+            'End-effector pose estimation from IMU data. \n (90 degree CW turn) ', fontsize=12)
+
+    def input_data(self):
+        try:
+            self.csv_file = sys.argv[1]
+            if len(sys.argv) > 2:
+                if sys.argv[2] == '--save':
+                    self.output = True
+        except (IndexError, TypeError):
+            print("Check csv file path.")
+            sys.exit(1)
+
+    def csv_to_pandas(self):
+        try:
+            self.imu_data = pd.read_csv(self.csv_file)
+        except FileNotFoundError:
+            print("Unable to read provided file.")
+
+    def genarator(self, index):
+        try:
+            yield self.imu_data.iloc[index]
+        except KeyError:
+            plt.pause(15)
+            sys.exit(1)
+
+    def plot_acc(self):
+        ax = self.axs[0, 0]
+        seaborn.lineplot(ax=self.axs[0, 0], data=self.data[[
+                         'acc_x', 'acc_y', 'acc_z']]).set_title("Accelerometer")
+
+    def plot_gyro(self):
+        seaborn.lineplot(ax=self.axs[1, 0], data=self.data[[
+                         'gyro_x', 'gyro_y', 'gyro_z']]).set_title("Gyroscope")
+
+    def plot_mag(self):
+        seaborn.lineplot(ax=self.axs[1, 1], data=self.data[[
+                         'mag_x', 'mag_y', 'mag_z']]).set_title("Magnetometer")
+
+    '''TODO'''
+
+    def plot_end_effector_pose(self):
+        # end-effector pose estimation results
+        self.axs[0, 1].set_title("end-effector")
+        self.axs[0, 1].set_xlim([0, 500])
+        self.axs[0, 1].set_ylim([500, 0])
+        self.axs[0, 1].set_zlim([0, 500])
+
+        # initial point (x, y, z) (265, 15, 375) plot (x, y, z) => robot (z, x, y)
+        self.axs[0, 1].scatter(265, 375, 15, color='b')
+
+        # base location
+        # self.axs[0, 1].scatter(250, 250, 470)
+
+    def save_video(slef, anim):
+        writer = animation.FFMpegWriter(fps=10)
+        anim.save('imu_pose_estimation.mp4', writer=writer)
+
+    def update(self, index):
+        g = next(self.genarator(index))
+
+        # clear prev plots
+        self.axs[0, 0].clear()
+        self.axs[0, 1].clear()
+        self.axs[1, 0].clear()
+        self.axs[1, 1].clear()
+
+        # update data
+        new_row = {'temp': g.temp,
+                   'acc_x': g.acc_x, 'acc_y': g.acc_y, 'acc_z': g.acc_z,
+                   'gyro_x': g.gyro_x + self.gyro_calibration['gyro_x'],
+                   'gyro_y': g.gyro_y + self.gyro_calibration['gyro_y'],
+                   'gyro_z': g.gyro_z + self.gyro_calibration['gyro_z'],
+                   'mag_x': g.mag_x, 'mag_y': g.mag_y, 'mag_z': g.mag_z,
+                   'time': g.time}
+
+        self.data = self.data.append(new_row, ignore_index=True)
+
+        # create new plots
+        self.plot_acc()
+        self.plot_gyro()
+        self.plot_mag()
+        self.plot_end_effector_pose()
+
+        self.index += 1
+
+    def main(self):
+        anim = animation.FuncAnimation(plt.gcf(), self.update, frames=1000,
+                                       interval=10, blit=False, repeat=False)
+        if self.output == False:
+            plt.show()
+        elif self.output == True:
+            plt.close()
+            self.save_video(anim)
+
+
+if __name__ == '__main__':
+    PoseEstimationIMU().main()