Setup LaTeX and cross-compile

.gitignore

@@ -134,3 +134,13 @@ cython_debug/
 
 # Ignore exercises
 exercises/
+
+# LaTeX Garbage
+*.aux
+*.fdb_latexmk
+*.fls
+*.log
+*.synctex.gz
+
+# LaTeX output
+*.pdf

Modcon.code-workspace

@@ -0,0 +1,8 @@
+{
+	"folders": [
+		{
+			"path": "."
+		}
+	],
+	"settings": {}
+}

doc/pendulum.tex

@@ -0,0 +1,69 @@
+% This is a simple sample document.  For more complicated documents take a look in the exercise tab. Note that everything that comes after a % symbol is treated as comment and ignored when the code is compiled.
+
+\documentclass{article} % \documentclass{} is the first command in any LaTeX code.  It is used to define what kind of document you are creating such as an article or a book, and begins the document preamble
+
+\usepackage[a4paper,top=2cm,bottom=2cm,left=2cm,right=2cm,marginparwidth=1.75cm]{geometry}
+\usepackage{amsmath} % \usepackage is a command that allows you to add functionality to your LaTeX code
+\usepackage{multicol}
+\usepackage{textcomp}
+\usepackage{graphicx}
+\usepackage{float}
+
+\title{
+    Single inverted pendulum\\~\
+    \large{Modeling and Control course}
+}
+
+\author{Arne van Iterson} % Sets authors name
+\date{\today} % Sets date for date compiled
+
+% The preamble ends with the command \begin{document}
+\begin{document} % All begin commands must be paired with an end command somewhere
+\maketitle % creates title using information in preamble (title, author, date)
+
+
+\begin{abstract}
+    Very interesting Introduction
+
+\end{abstract}
+
+\begin{multicols}{2} % creates two columns
+    % Theory 
+    \section{Theory}
+    The system that will be discussed in this paper is a single inverted pendulum. Common implementations of this system include a cart, this is not the case in this paper. 
+
+    \begin{figure}[H]
+        \includegraphics[width=\linewidth]{../res/segway.jpg}
+        \caption{Segway\textsuperscript{\tiny\textregistered} in use}
+        \label{fig:segway}
+    \end{figure}
+
+    A practical example of this system is a Segway\textsuperscript{\tiny\textregistered} (figure \ref{fig:segway}). The system is inherently unstable, meaning that if the pendulum is not controlled, it will fall over. 
+    
+    % Hardware setup
+    \section{Setup}
+    To simplify hardware design, the system will be built and run on a LEGO\textsuperscript{\tiny\textregistered} Mindstorms EV3 development kit. The programmable brick itself, which will be referred to as "EV3", runs a Linux distribution known as ev3dev. Ev3dev allows for various programming languages to be used including Python and C. Both of which will be further explained later. 
+
+    To reproduce the robot in Figure \ref{fig:balancer}, please refer to the building instructions in the appendix.
+
+    \begin{figure}[H]
+        \includegraphics[width=\linewidth]{../res/bricklink/modcon_balancer2.png}
+        \caption{Balancing robot}
+        \label{fig:balancer}
+    \end{figure}
+
+\section{Model}
+\subsection{Linearisation}
+
+\section{Control}
+dsaf
+
+\section{Validation}
+fsafdfa
+
+\section{Conclusion}
+asfjdklasdfas
+
+\end {multicols}
+
+\end{document} % This is the end of the document

src/ev3.py

@@ -0,0 +1,26 @@
+#!/usr/bin/env python3
+import os
+import time 
+
+from ev3dev2.motor import LargeMotor, OUTPUT_B, OUTPUT_C, SpeedPercent, MoveTank
+from ev3dev2.sensor import INPUT_2
+from ev3dev2.sensor.lego import GyroSensor
+from ev3dev2.led import Leds
+
+os.system('setfont Lat15-TerminusBold14')
+
+# tank_drive = MoveTank(OUTPUT_B,OUTPUT_C)
+
+gyro = GyroSensor(INPUT_2)
+
+dt = 0
+t = time.time()
+
+while (True):
+    # Timekeepers
+    dt = time.time() - t
+    t = time.time()
+
+    # Gyro
+    gyro_value = gyro.angle
+    print("Gyro: " + str(gyro_value))

src/ev3/CMakeLists.txt

@@ -0,0 +1,23 @@
+# CMake version
+cmake_minimum_required(VERSION 3.5)
+
+# Setup cross-compile
+set(CMAKE_C_COMPILER "arm-linux-gnueabi-gcc")
+set(CMAKE_CXX_COMPILER "arm-linux-gnueabi-g++")
+set(CMAKE_TRY_COMPILE_TARGET_TYPE STATIC_LIBRARY)
+
+# Project name
+project(
+    ModconC
+    VERSION 1.0
+    LANGUAGES C CXX)
+
+# EV3 dev library
+set(EV3LIBPATH ${CMAKE_SOURCE_DIR}/lib/ev3/)
+file(GLOB EV3SRCFILES ${EV3LIBPATH}/*.h ${EV3LIBPATH}/*.c)
+add_library(ev3lib ${EV3SRCFILES})
+
+# Main executable
+add_executable("main.elf" src/main.c)
+target_include_directories("main.elf" PUBLIC ${EV3LIBPATH})
+target_link_libraries("main.elf" ev3lib)

src/ev3/cc.ps1

@@ -0,0 +1,9 @@
+# Define your Docker container name
+$volumeName = "ev3cc"
+$containerName = "modconcc"
+
+# Run a command in the container and capture the output
+$commandOutput = docker run --rm --name $containerName -v C:/Users/User/Documents/Git/Modcon/src/ev3:/src -w /src/build $volumeName /bin/bash -c "cmake .. && make"
+
+# Display the command output
+Write-Output $commandOutput

src/ev3/cli.ps1

@@ -1 +1 @@
-docker run --rm -it -v C:/Users/User/Documents/Git/Modcon/src/ev3:/src -w /src ev3cc
+docker run --rm -it -v C:/Users/User/Documents/Git/Modcon/src/ev3:/src -w /src ev3cc

src/ev3/deploy.ps1

@@ -0,0 +1,10 @@
+$ip = "ev3dev.local"
+$user = "robot"
+$key = $env:USERPROFILE + "\.ssh\id_rsa"
+
+echo "Compile"
+.\cc.ps1
+
+echo "Deploy"
+scp -i $key .\build\main.elf ${user}@${ip}:/home/robot/main.elf
+ssh -i $key -t ${user}@${ip} "chmod +x /home/robot/main.elf"

src/ev3/src/main.c

@@ -0,0 +1,44 @@
+#include <stdio.h>
+#include <time.h>
+#include "ev3.h"
+#include "ev3_light.h"
+#include "ev3_port.h"
+#include "ev3_tacho.h"
+
+int main(void)
+{
+    int i;
+    char s[256];
+    uint8_t sn;
+
+    ev3_init();
+
+    set_light(LIT_LEFT, LIT_AMBER);
+
+    while (ev3_tacho_init() < 1)
+        ;
+
+    printf("Found tacho motors:\n");
+
+    for (i = 0; i < DESC_LIMIT; i++)
+    {
+        if (ev3_tacho[i].type_inx != TACHO_TYPE__NONE_)
+        {
+            printf("  type = %s\n", ev3_tacho_type(ev3_tacho[i].type_inx));
+            printf("  port = %d, %s\n", i, ev3_tacho_port_name(i, s));
+
+            int max_speed;
+            get_tacho_max_speed(i, &max_speed);
+            printf("max speed = %d\n", max_speed);
+            set_tacho_stop_action_inx(i, TACHO_COAST);
+            set_tacho_speed_sp(i, max_speed * 2 / 3);
+            set_tacho_time_sp(i, 1000);
+            set_tacho_ramp_up_sp(i, 0);
+            set_tacho_ramp_down_sp(i, 0);
+            set_tacho_command_inx(i, TACHO_RUN_TIMED);
+        }
+    }
+
+    ev3_uninit();
+    return 0;
+}

src/htway.py

@@ -0,0 +1,240 @@
+#!/usr/bin/env python
+
+# Python port of the HiTechnic HTWay for ev3dev
+# Copyright (c) 2014-2015 G33kDude, David Lechner
+# HiTechnic HTWay is Copyright (c) 2010 HiTechnic
+import itertools
+import os
+import struct
+import time
+import pyudev
+
+# loop interval in seconds
+WAIT_TIME = 0.008
+
+# ratio of wheel size compared to NXT 1.0
+WHEEL_RATIO_NXT1 = 1.0 # 56mm
+WHEEL_RATIO_NXT = 0.8 # 43.2mm (same as EV3)
+WHEEL_RATIO_RCX = 1.4 # 81.6mm
+
+# These are the main four balance constants, only the gyro constants
+# are relative to the wheel size. KPOS and KSPEED are self-relative to
+# the wheel size.
+KGYROANGLE = 7.5
+KGYROSPEED = 1.15
+KPOS = 0.07
+KSPEED = 0.1
+
+# This constant aids in drive control. When the robot starts moving
+# because of user control, this constant helps get the robot leaning in
+# the right direction. Similarly, it helps bring robot to a stop when
+# stopping.
+KDRIVE = -0.02
+
+# Power differential used for steering based on difference of target
+# steering and actual motor difference.
+KSTEER = 0.25
+
+# If robot power is saturated (over +/- 100) for over this time limit
+# then robot must have fallen.  In seconds.
+TIME_FALL_LIMIT = 0.5
+
+# Gyro offset control
+# The HiTechnic gyro sensor will drift with time.  This constant is
+# used in a simple long term averaging to adjust for this drift.
+# Every time through the loop, the current gyro sensor value is
+# averaged into the gyro offset weighted according to this constant.
+EMAOFFSET = 0.0005
+
+class Gyro:
+    @staticmethod
+    def get_gyro():
+        devices = list(pyudev.Context().list_devices(subsystem='lego-sensor') \
+                .match_property("LEGO_DRIVER_NAME", EV3Gyro.DRIVER_NAME) \
+                .match_property("LEGO_DRIVER_NAME", HTGyro.DRIVER_NAME))
+
+        if not devices:
+            raise Exception('Gyro not found')
+
+        if devices[0].attributes.get('driver_name') == EV3Gyro.DRIVER_NAME:
+            return EV3Gyro(devices[0])
+        elif devices[0].attributes.get('driver_name') == HTGyro.DRIVER_NAME:
+            return HTGyro(devices[0])
+
+    def __init__(self, device):
+        self.device = device
+        self.value0 = open(os.path.join(self.device.sys_path, 'value0'), 'r', 0)
+        self.offset = 0.0
+        self.angle = 0.0
+
+    def calibrate(self):
+        self.angle = 0.0
+        total = 0
+        for i in range(10):
+            total += self.get_rate()
+            time.sleep(0.01)
+        average = total / 10.0
+        self.offset = average - 0.1
+
+    def set_mode(self, value):
+        with open(os.path.join(self.device.sys_path, 'mode'), 'w') as f:
+            f.write(str(value))
+
+    def get_rate(self):
+        self.value0.seek(0)
+        return int(self.value0.read())
+
+class EV3Gyro(Gyro):
+    DRIVER_NAME = 'lego-ev3-gyro'
+
+    def __init__(self, device):
+        Gyro.__init__(self, device)
+        self.set_mode("GYRO-RATE")
+
+    def get_data(self, interval):
+        gyro_raw = self.get_rate()
+        self.offset = EMAOFFSET * gyro_raw + (1 - EMAOFFSET) * self.offset
+        speed = (gyro_raw - self.offset)
+        self.angle += speed * interval
+        return speed, self.angle
+
+class HTGyro(Gyro):
+    DRIVER_NAME = 'ht-nxt-gyro'
+
+    def __init__(self, device):
+        Gyro.__init__(self, device)
+
+    def get_data(self, interval):
+        gyro_raw = self.get_rate()
+        self.offset = EMAOFFSET * gyro_raw + (1 - EMAOFFSET) * self.offset
+        speed = (gyro_raw - self.offset) * 400.0 / 1953.0
+        self.angle += speed * interval
+        return speed, self.angle
+
+class EV3Motor:
+    def __init__(self, which=0):
+        devices = list(pyudev.Context().list_devices(subsystem='tacho-motor').match_attribute('driver_name', 'lego-ev3-l-motor'))
+
+        if which >= len(devices):
+            raise Exception("Motor not found")
+
+        self.device = devices[which]
+        self.pos = open(os.path.join(self.device.sys_path, 'position'), 'r+',0)
+        self.duty_cycle_sp = open(os.path.join(self.device.sys_path,'duty_cycle_sp'), 'w', 0)
+        self.reset()
+
+    def reset(self):
+        self.set_pos(0)
+        self.set_duty_cycle_sp(0)
+        self.send_command("run-direct")
+        
+    def get_pos(self):
+        self.pos.seek(0)
+        return int(self.pos.read())
+
+    def set_pos(self, new_pos):
+        return self.pos.write(str(int(new_pos)))
+
+    def set_duty_cycle_sp(self, new_duty_cycle_sp):
+        return self.duty_cycle_sp.write(str(int(new_duty_cycle_sp)))
+
+    def send_command(self, new_mode):
+        with open(os.path.join(self.device.sys_path, 'command'),'w') as command:
+            command.write(str(new_mode))
+
+class EV3Motors:
+    def __init__(self, left=0, right=1):
+        self.left = EV3Motor(left)
+        self.right = EV3Motor(right)
+        self.pos = 0.0
+        self.speed = 0.0
+        self.diff = 0
+        self.target_diff = 0
+        self.drive = 0
+        self.steer = 0
+        self.prev_sum = 0
+        self.prev_deltas = [0,0,0]
+
+    def get_data(self, interval):
+        left_pos = self.left.get_pos()
+        right_pos = self.right.get_pos()
+
+        pos_sum = right_pos + left_pos
+        self.diff = left_pos - right_pos
+
+        delta = pos_sum - self.prev_sum
+        self.pos += delta
+
+        self.speed = (delta + sum(self.prev_deltas)) / (4 * interval)
+
+        self.prev_sum = pos_sum
+        self.prev_deltas = [delta] + self.prev_deltas[0:2]
+
+        return self.speed, self.pos
+
+    def steer_control(self, power, steering, interval):
+        self.target_diff += steering * interval
+        power_steer = KSTEER * (self.target_diff - self.diff)
+        power_left = max(-100, min(power + power_steer, 100))
+        power_right = max(-100, min(power - power_steer, 100))
+        return power_left, power_right
+
+
+def main():
+    wheel_ratio = WHEEL_RATIO_NXT
+
+    gyro = Gyro.get_gyro()
+    gyro.calibrate()
+
+    print("balancing in ...")
+    for i in range(5, 0, -1):
+        print(i)
+        os.system("beep -f 440 -l 100")
+        time.sleep(1)
+    print(0)
+
+    os.system("beep -f 440 -l 1000")
+    time.sleep(1)
+
+    motors = EV3Motors()
+    start_time = time.time()
+    last_okay_time = start_time
+
+    avg_interval = 0.0055
+    
+    for loop_count in itertools.count():
+        gyro_speed, gyro_angle = gyro.get_data(avg_interval)
+        motors_speed, motors_pos = motors.get_data(avg_interval)
+        #print gyro_speed, gyro_angle, motors_speed, motors_pos
+        motors_pos -= motors.drive * avg_interval
+        motors.pos = motors_pos
+
+        power = (KGYROSPEED * gyro_speed
+                + KGYROANGLE * gyro_angle) \
+                / wheel_ratio \
+                + KPOS * motors_pos \
+                + KDRIVE * motors.drive \
+                + KSPEED * motors_speed
+
+        left_power, right_power = motors.steer_control(power, 0, avg_interval)
+
+        #print left_power, right_power
+
+        motors.left.set_duty_cycle_sp(left_power)
+        motors.right.set_duty_cycle_sp(right_power)
+
+        time.sleep(WAIT_TIME)
+
+        now_time = time.time()
+        avg_interval = (now_time - start_time) / (loop_count+1)
+
+        if abs(power) < 100:
+            last_okay_time = now_time
+        elif now_time - last_okay_time > TIME_FALL_LIMIT:
+            break
+
+    motors.left.send_command('reset')
+    motors.right.send_command('reset')
+
+if __name__ == "__main__":
+    main()

src/sim/sim.py

@@ -52,6 +52,8 @@ def meta():
     ui.meta(pendulum.kd, "Kd")
     ui.meta(pendulum.ki, "Ki")
     ui.meta(pendulum.kp, "Kp")
+    
+    ui.meta(dt, "dt")
 
     ui.meta(not update, "Paused")
     ui.meta(rt / 1000, "Run time [s]")