diff --git a/.gitignore b/.gitignore index 35289ac..c4bc088 100644 --- a/.gitignore +++ b/.gitignore @@ -134,3 +134,13 @@ cython_debug/ # Ignore exercises exercises/ + +# LaTeX Garbage +*.aux +*.fdb_latexmk +*.fls +*.log +*.synctex.gz + +# LaTeX output +*.pdf \ No newline at end of file diff --git a/Modcon.code-workspace b/Modcon.code-workspace new file mode 100644 index 0000000..876a149 --- /dev/null +++ b/Modcon.code-workspace @@ -0,0 +1,8 @@ +{ + "folders": [ + { + "path": "." + } + ], + "settings": {} +} \ No newline at end of file diff --git a/deploy.ps1 b/doc/ethics.tex similarity index 100% rename from deploy.ps1 rename to doc/ethics.tex diff --git a/doc/pendulum.tex b/doc/pendulum.tex new file mode 100644 index 0000000..255ac1a --- /dev/null +++ b/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 diff --git a/res/bricklink/modcon_balancer.io b/res/bricklink/modcon_balancer.io new file mode 100644 index 0000000..76239fc Binary files /dev/null and b/res/bricklink/modcon_balancer.io differ diff --git a/res/bricklink/modcon_balancer.png b/res/bricklink/modcon_balancer.png new file mode 100644 index 0000000..ab65863 Binary files /dev/null and b/res/bricklink/modcon_balancer.png differ diff --git a/res/bricklink/modcon_balancer2.png b/res/bricklink/modcon_balancer2.png new file mode 100644 index 0000000..378d850 Binary files /dev/null and b/res/bricklink/modcon_balancer2.png differ diff --git a/res/segway.jpg b/res/segway.jpg new file mode 100644 index 0000000..306cfde Binary files /dev/null and b/res/segway.jpg differ diff --git a/src/ev3.py b/src/ev3.py index e69de29..f6506c2 100644 --- a/src/ev3.py +++ b/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)) diff --git a/src/ev3/CMakeLists.txt b/src/ev3/CMakeLists.txt new file mode 100644 index 0000000..0bc1914 --- /dev/null +++ b/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) diff --git a/src/ev3/cc.ps1 b/src/ev3/cc.ps1 new file mode 100644 index 0000000..62850ac --- /dev/null +++ b/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 \ No newline at end of file diff --git a/cc.ps1 b/src/ev3/cli.ps1 similarity index 75% rename from cc.ps1 rename to src/ev3/cli.ps1 index 6044eeb..5a30b27 100644 --- a/cc.ps1 +++ b/src/ev3/cli.ps1 @@ -1 +1 @@ -docker run --rm -it -v C:/Users/User/Documents/Git/Modcon/src/ev3:/src -w /src ev3cc \ No newline at end of file +docker run --rm -it -v C:/Users/User/Documents/Git/Modcon/src/ev3:/src -w /src ev3cc diff --git a/src/ev3/deploy.ps1 b/src/ev3/deploy.ps1 new file mode 100644 index 0000000..50e07f1 --- /dev/null +++ b/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" \ No newline at end of file diff --git a/src/ev3/src/main.c b/src/ev3/src/main.c new file mode 100644 index 0000000..2e36f54 --- /dev/null +++ b/src/ev3/src/main.c @@ -0,0 +1,44 @@ +#include +#include +#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; +} \ No newline at end of file diff --git a/src/gyro.py b/src/gyro.py new file mode 100644 index 0000000..e69de29 diff --git a/src/htway.py b/src/htway.py new file mode 100644 index 0000000..ee8c99e --- /dev/null +++ b/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() \ No newline at end of file diff --git a/src/sim/sim.py b/src/sim/sim.py index 7db4a81..eabaaf4 100644 --- a/src/sim/sim.py +++ b/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]")