├── xbox.png ├── .gitignore ├── dc-socket.jpg ├── dfrobot.jpg ├── profile.png ├── redboard.jpg ├── rollpaw.jpg ├── 6axis-size.jpg ├── schematics.jpg ├── usb-mini-b.jpg ├── jr-servo-wire.jpg ├── power-supply.jpg ├── TODO.md ├── arduino ├── Makefile └── sainsmart.ino ├── joystick.rb ├── calibration.hh.default ├── .travis.yml ├── etc ├── curve.dem └── profile.dem ├── profile.hh ├── Makefile ├── COPYING ├── matrix_ext.rb ├── serial_client.rb ├── path.hh ├── spec ├── matrix_ext_spec.rb ├── serial_client_spec.rb ├── kinematics_spec.rb └── control_spec.rb ├── control.rb ├── kinematics.rb ├── README.md ├── controllerbase.hh └── test-suite.cc /xbox.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wedesoft/arduino-sainsmart/HEAD/xbox.png -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | test-suite 2 | calibration.hh 3 | build-* 4 | *.o 5 | .*.swp 6 | .*.un~ 7 | -------------------------------------------------------------------------------- /dc-socket.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wedesoft/arduino-sainsmart/HEAD/dc-socket.jpg -------------------------------------------------------------------------------- /dfrobot.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wedesoft/arduino-sainsmart/HEAD/dfrobot.jpg -------------------------------------------------------------------------------- /profile.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wedesoft/arduino-sainsmart/HEAD/profile.png -------------------------------------------------------------------------------- /redboard.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wedesoft/arduino-sainsmart/HEAD/redboard.jpg -------------------------------------------------------------------------------- /rollpaw.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wedesoft/arduino-sainsmart/HEAD/rollpaw.jpg -------------------------------------------------------------------------------- /6axis-size.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wedesoft/arduino-sainsmart/HEAD/6axis-size.jpg -------------------------------------------------------------------------------- /schematics.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wedesoft/arduino-sainsmart/HEAD/schematics.jpg -------------------------------------------------------------------------------- /usb-mini-b.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wedesoft/arduino-sainsmart/HEAD/usb-mini-b.jpg -------------------------------------------------------------------------------- /jr-servo-wire.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wedesoft/arduino-sainsmart/HEAD/jr-servo-wire.jpg -------------------------------------------------------------------------------- /power-supply.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/wedesoft/arduino-sainsmart/HEAD/power-supply.jpg -------------------------------------------------------------------------------- /TODO.md: -------------------------------------------------------------------------------- 1 | # TODO 2 | 3 | * rotations, gripper 4 | * acos out of range 5 | * teach points on computer (create program on stdout) 6 | * speech control 7 | * audio output 8 | * machine vision 9 | * servos with feedback 10 | * visualise nominal vs actual position 11 | * instructions (screw terminal) 12 | -------------------------------------------------------------------------------- /arduino/Makefile: -------------------------------------------------------------------------------- 1 | ARDUINO_DIR = /usr/share/arduino 2 | ARDMK_DIR = /usr/share/arduino 3 | BOARD_TAG = uno 4 | BOARD_SUB = atmega168 5 | ARDUINO_PORT = /dev/ttyUSB0 6 | ARDUINO_LIBS = Servo 7 | SCREEN = screen 8 | 9 | include $(ARDMK_DIR)/Arduino.mk 10 | 11 | $(OBJDIR)/sainsmart.o: ../profile.hh ../path.hh ../controllerbase.hh ../calibration.hh 12 | 13 | repl: 14 | $(SCREEN) $(ARDUINO_PORT) 115200 15 | -------------------------------------------------------------------------------- /joystick.rb: -------------------------------------------------------------------------------- 1 | require 'sdl2' 2 | 3 | 4 | class Joystick 5 | attr_reader :axis 6 | attr_reader :button 7 | 8 | def initialize 9 | SDL2.init SDL2::INIT_JOYSTICK 10 | @axis = {} 11 | @button = {} 12 | end 13 | 14 | def update 15 | while event = SDL2::Event.poll 16 | case event 17 | when SDL2::Event::JoyDeviceAdded 18 | @device = SDL2::Joystick.open event.which 19 | when SDL2::Event::JoyAxisMotion 20 | @axis[event.axis] = event.value 21 | when SDL2::Event::JoyButtonDown 22 | @button[event.button] = true 23 | when SDL2::Event::JoyButtonUp 24 | @button[event.button] = false 25 | end 26 | end 27 | end 28 | end 29 | -------------------------------------------------------------------------------- /calibration.hh.default: -------------------------------------------------------------------------------- 1 | #ifndef __CALIBRATION_HH 2 | #define __CALIBRATION_HH 3 | 4 | 5 | 6 | // BASE, SHOULDER, ELBOW, ROLL, PITCH, WRIST, GRIPPER (defined in controllerbase.hh) 7 | const int SERVOPIN[] = {11, 10, 9, 8, 7, 6, 5}; 8 | const int OFFSET[] = {1471, 1465, 700, 1240, 1550, 1366, 1333};// normally around 1500 except for elbow and gripper 9 | 10 | #warning "Adjust these values for your robot to avoid self-collisions!" 11 | #warning "Control your robot carefully until you have inserted appropriate boundaries here!" 12 | const int MIN[] = {544, 938, 638, 544, 544, 544, 1299};// must not be below 544 13 | const int MAX[] = {2400, 2400, 2400, 2400, 2253, 2400, 2222};// must not be above 2400 14 | 15 | const float RESOLUTION[] = {8.844444444444445, 8.844444444444445, 8.844444444444445, 8.844444444444445, 11.433333333333334, 10.133333333333333, 10.133333333333333}; 16 | const float MAXJERK = 250; 17 | 18 | #endif 19 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | sudo: required 2 | dist: xenial 3 | language: c 4 | compiler: 5 | - gcc 6 | before_install: 7 | - sudo apt-get install arduino-mk 8 | - sudo apt-get install ruby ruby-dev ruby-serialport 9 | - sudo apt-get install libsdl2-dev libsdl2-gfx-dev libsdl2-image-dev libsdl2-mixer-dev libsdl2-net-dev libsdl2-ttf-dev 10 | - sudo gem install rspec 11 | - sudo gem install ruby-sdl2 12 | - wget -c https://github.com/google/googletest/archive/release-1.7.0.tar.gz -O googletest-release-1.7.0.tar.gz 13 | - tar xzf googletest-release-1.7.0.tar.gz 14 | - wget -c https://github.com/google/googlemock/archive/release-1.7.0.tar.gz -O googlemock-release-1.7.0.tar.gz 15 | - tar xzf googlemock-release-1.7.0.tar.gz 16 | install: 17 | - cp calibration.hh.default calibration.hh 18 | - env PATH=/usr/share/arduino:$PATH make all-recursive 19 | script: 20 | - make GTEST=googletest-release-1.7.0 GMOCK=googlemock-release-1.7.0 check 21 | -------------------------------------------------------------------------------- /etc/curve.dem: -------------------------------------------------------------------------------- 1 | line(t):=x*t/d; 2 | fixspeed1(t,c1):=c1*t*(t-d)^2; 3 | fixspeed2(t,c2):=c2*t^2*(t-d); 4 | fixacceleration1(t,c3):=c3*t^2*(t-d)^3; 5 | fixacceleration2(t,c4):=c4*t^3*(t-d)^2; 6 | position(t,c1,c2,c3,c4) := line(t)+fixspeed1(t,c1)+fixspeed2(t,c2)+fixacceleration1(t,c3)+fixacceleration2(t,c4); 7 | speed(t,c1,c2,c3,c4) := diff(position(t,c1,c2,c3,c4),t); 8 | acceleration(t,c1,c2,c3,c4) := diff(speed(t,c1,c2,c3,c4),t); 9 | solve(subst([t=0],speed(t,c1,c2,c3,c4)),c1); 10 | solve(subst([t=d],speed(t,c1,c2,c3,c4)),c2); 11 | solve(subst([t=0,c1=-x/d^3,c2=-x/d^3],acceleration(t,c1,c2,c3,c4)),c3); 12 | solve(subst([t=d,c1=-x/d^3,c2=-x/d^3],acceleration(t,c1,c2,c3,c4)),c4); 13 | 14 | p(t,x,d):=ratexpand(position(t,-x/d^3,-x/d^3,3*x/d^5,3*x/d^5)); 15 | v(t,x,d):=diff(p(t,x,d),t); 16 | a(t,x,d):=diff(v(t,x,d),t); 17 | j(t,x,d):=diff(a(t,x,d),t); 18 | u(t,x,d):=diff(j(t,x,d),t); 19 | 20 | solve(u(t,x,d),t); 21 | j(t,x,d):=360*t^2*x/d^5-360*t*x/d^4+60*x/d^3; 22 | j(0,x,d); j(d/2,x,d); j(d,x,d); 23 | solve(60*x/d^3-dmax,d); 24 | d=(60*x/dmax)^(1/3); 25 | 26 | plot2d([p(t,1,2.4),v(t,1,2.4),a(t,1,2.4),j(t,1,2.4)],[t,0,2.4]); 27 | plot2d([p(t,1,1)],[t,0,1]); 28 | -------------------------------------------------------------------------------- /profile.hh: -------------------------------------------------------------------------------- 1 | #ifndef __PROFILE_HH 2 | #define __PROFILE_HH 3 | 4 | #include 5 | 6 | class Profile 7 | { 8 | public: 9 | Profile(void) { reset(); } 10 | Profile(float distance, float duration) { 11 | reset(distance, duration); 12 | } 13 | void reset(void) { 14 | m_distance = 0; 15 | m_duration = 0; 16 | } 17 | void reset(float distance, float duration) { 18 | m_distance = distance; 19 | m_duration = duration; 20 | }; 21 | float distance(void) { return m_distance; } 22 | float duration(void) { return m_duration; } 23 | bool empty(void) { return m_duration == 0; } 24 | float value(float time) { 25 | float retval; 26 | if (time > 0) 27 | if (time < m_duration) 28 | retval = m_distance * (M_PI * time / m_duration - 0.5 * sin(2 * M_PI * time / m_duration)) / M_PI; 29 | else 30 | retval = m_distance; 31 | else 32 | retval = 0; 33 | return retval; 34 | } 35 | static float timeRequired(float distance, float maxJerk) { 36 | return cbrtf(4 * M_PI * M_PI * distance / maxJerk); 37 | } 38 | protected: 39 | float m_distance; 40 | float m_duration; 41 | }; 42 | 43 | #endif 44 | -------------------------------------------------------------------------------- /Makefile: -------------------------------------------------------------------------------- 1 | .SUFFIXES: .rb 2 | 3 | GTEST=/usr/src/googletest/googletest 4 | GMOCK=/usr/src/googletest/googlemock 5 | CXX = g++ 6 | RSPEC = rspec 7 | RM_F = rm -f 8 | 9 | all: all-recursive 10 | 11 | check: check-controller check-client 12 | 13 | check-controller: test-suite 14 | ./test-suite 15 | 16 | check-client: 17 | $(RSPEC) 18 | 19 | upload: 20 | cd arduino && $(MAKE) upload && cd .. 21 | 22 | repl: 23 | cd arduino && $(MAKE) repl && cd .. 24 | 25 | test-suite: test-suite.o gtest-all.o gmock-all.o 26 | $(CXX) -o $@ test-suite.o gtest-all.o gmock-all.o -lpthread 27 | 28 | test-suite.o: test-suite.cc controllerbase.hh calibration.hh profile.hh path.hh 29 | $(CXX) -c -I$(GMOCK)/include -I$(GTEST)/include -o $@ $< 30 | 31 | gtest-all.o: $(GTEST)/src/gtest-all.cc 32 | $(CXX) -c -I$(GTEST)/include -I$(GTEST) -o $@ $< 33 | 34 | gmock-all.o: $(GMOCK)/src/gmock-all.cc 35 | $(CXX) -c -I$(GMOCK)/include -I$(GTEST)/include -I$(GMOCK) -o $@ $< 36 | 37 | clean: clean-recursive clean-local 38 | 39 | clean-local: 40 | $(RM_F) -f test-suite *.o 41 | 42 | all-recursive: 43 | cd arduino && $(MAKE) && cd .. 44 | 45 | clean-recursive: 46 | cd arduino && $(MAKE) clean && cd .. 47 | -------------------------------------------------------------------------------- /COPYING: -------------------------------------------------------------------------------- 1 | Copyright (c) 2015, Jan Wedekind 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions are met: 5 | 6 | 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. 7 | 8 | 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the documentation and/or other materials provided with the distribution. 9 | 10 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 11 | -------------------------------------------------------------------------------- /etc/profile.dem: -------------------------------------------------------------------------------- 1 | /* mathematical derivation of position profile and profile duration 2 | * given: 3 | * l is the distance to travel 4 | * t is time 5 | * d is duration 6 | * c is a constant factor 7 | * m is the maximum jerk 8 | * v and vv is speed 9 | * s and ss is position 10 | * a and aa is acceleration 11 | * j and jj is jerk 12 | */ 13 | 14 | /* use sin(t)^2 for speed profile */ 15 | v(t, d, c) := c * sin(%pi * t / d) ^ 2; 16 | plot2d(v(t, 10, 1), [t, 0, 10]); 17 | 18 | /* determine position */ 19 | integrate(v(t, d, c), t); 20 | s(t, d, c) := (c * d * (%pi * t / d - sin(2 * %pi * t / d) / 2)) / (2 * %pi); 21 | plot2d(s(t, 10, 1), [t, 0, 10]); 22 | 23 | /* determine constant factor */ 24 | solve(s(d, d, c) = l, c); 25 | 26 | /* resulting position */ 27 | ss(t, d, l) := s(t, d, 2 * l / d); 28 | ss(t, d, l) := l * (%pi * t / d - sin(2 * %pi * t / d) / 2) / %pi; 29 | 30 | /* resulting speed */ 31 | diff(ss(t, d, l), t); 32 | vv(t, d, l) := l * (%pi / d - %pi * cos(2 * %pi * t / d) / d) / %pi; 33 | 34 | /* resulting acceleration */ 35 | diff(vv(t, d, l), t); 36 | aa(t, d, l) := 2 * %pi * l * sin(2 * %pi * t / d) / d ^ 2; 37 | 38 | /* resulting jerk */ 39 | diff(aa(t, d, l), t); 40 | jj(t, d, l) := 4 * %pi ^ 2 * l * cos(2 * %pi * t / d) / d ^ 3; 41 | 42 | /* determine duration given maximum jerk */ 43 | solve(jj(0, d, l) = m, d); 44 | d = (4 * %pi ^ 2 * l / m) ^ (1 / 3); 45 | 46 | plot2d([jj(t, 4, 1), aa(t, 4, 1), vv(t, 4, 1), ss(t, 4, 1)], [t, 0, 4]); 47 | -------------------------------------------------------------------------------- /matrix_ext.rb: -------------------------------------------------------------------------------- 1 | require 'matrix' 2 | 3 | 4 | class Vector 5 | def abs 6 | Math.sqrt inject(0) { |s, x| s + x ** 2 } 7 | end 8 | end 9 | 10 | class Matrix 11 | class << self 12 | def rotate angle, i, j 13 | cos_angle, sin_angle = Math.cos(angle), Math.sin(angle) 14 | arr = Matrix.identity(4).to_a 15 | arr[i][i], arr[i][j] = cos_angle, -sin_angle 16 | arr[j][i], arr[j][j] = sin_angle, cos_angle 17 | Matrix[*arr] 18 | end 19 | 20 | def rotate_x angle 21 | rotate angle, 1, 2 22 | end 23 | 24 | def rotate_y angle 25 | rotate angle, 2, 0 26 | end 27 | 28 | def rotate_z angle 29 | rotate angle, 0, 1 30 | end 31 | 32 | def translation x, y, z 33 | arr = Matrix.identity(4).to_a 34 | arr[0][3] = x 35 | arr[1][3] = y 36 | arr[2][3] = z 37 | Matrix[*arr] 38 | end 39 | 40 | def translate_x distance 41 | translation distance, 0, 0 42 | end 43 | 44 | def translate_y distance 45 | translation 0, distance, 0 46 | end 47 | 48 | def translate_z distance 49 | translation 0, 0, distance 50 | end 51 | end 52 | 53 | def abs 54 | Math.sqrt inject { |s, x| s + x ** 2 } 55 | end 56 | 57 | 58 | def x 59 | column_vectors[0] 60 | end 61 | 62 | def y 63 | column_vectors[1] 64 | end 65 | 66 | def z 67 | column_vectors[2] 68 | end 69 | 70 | def translation 71 | column_vectors[3] 72 | end 73 | end 74 | -------------------------------------------------------------------------------- /serial_client.rb: -------------------------------------------------------------------------------- 1 | require 'serialport' 2 | 3 | class SerialClient 4 | def initialize device, baud 5 | @serial = SerialPort.new device, baud 6 | @serial.read_timeout = 2000 7 | end 8 | 9 | def time_required *values 10 | write_serial "#{values.collect { |value| "%3.1f" % value}.join ' '}t" 11 | read_serial.to_f 12 | end 13 | 14 | def time 15 | write_serial 't' 16 | read_serial.to_i * 0.001 17 | end 18 | 19 | def time_remaining 20 | write_serial 'T' 21 | read_serial.to_f 22 | end 23 | 24 | def target *values 25 | write_serial "#{values.collect { |value| "%3.1f" % value}.join ' '}c" 26 | end 27 | 28 | def configuration str 29 | write_serial str 30 | read_serial.split(' ').collect &:to_f 31 | end 32 | 33 | def pos 34 | configuration 'c' 35 | end 36 | 37 | def lower 38 | configuration 'l' 39 | end 40 | 41 | def upper 42 | configuration 'u' 43 | end 44 | 45 | def ready? 46 | write_serial 'o' 47 | read_serial.to_i != 0 48 | end 49 | 50 | def teach_point_name index 51 | (index + 'a'.ord).chr 52 | end 53 | 54 | def save_teach_point index 55 | write_serial "m#{teach_point_name(index)}" 56 | end 57 | 58 | def load_teach_point index 59 | c = teach_point_name index 60 | configuration "'#{c}d#{c}" 61 | end 62 | 63 | def stop 64 | write_serial 'x' 65 | end 66 | 67 | def write_serial str 68 | @serial.write str 69 | @serial.flush 70 | end 71 | 72 | def read_serial 73 | result = @serial.readline 74 | result 75 | end 76 | end 77 | 78 | -------------------------------------------------------------------------------- /path.hh: -------------------------------------------------------------------------------- 1 | #ifndef __PATH_HH 2 | #define __PATH_HH 3 | 4 | #include "profile.hh" 5 | 6 | class Path 7 | { 8 | public: 9 | Path(void): m_offset(0) { 10 | m_time[0] = 0; 11 | m_time[1] = 0; 12 | } 13 | float pos(void) { 14 | return m_offset + m_profile[0].value(m_time[0]) + m_profile[1].value(m_time[1]); 15 | } 16 | float update(float dt) { 17 | update(dt, m_time[0], m_profile[0]); 18 | update(dt, m_time[1], m_profile[1]); 19 | return pos(); 20 | } 21 | void update(float dt, float &time, Profile &profile) { 22 | time += dt; 23 | if (time >= profile.duration()) { 24 | m_offset += profile.distance(); 25 | profile.reset(); 26 | }; 27 | } 28 | void stop(float pos) { 29 | m_offset = pos; 30 | m_profile[0].reset(); 31 | m_profile[1].reset(); 32 | } 33 | bool ready(void) { 34 | return m_profile[0].empty() || m_profile[1].empty(); 35 | } 36 | void retarget(float target, float duration) { 37 | retarget(target, duration, m_time[0], m_profile[0]) || retarget(target, duration, m_time[1], m_profile[1]); 38 | } 39 | float target(void) { 40 | return m_offset + m_profile[0].distance() + m_profile[1].distance(); 41 | } 42 | bool retarget(float value, float duration, float &time, Profile &profile) { 43 | if (!profile.empty()) 44 | return false; 45 | else { 46 | time = 0; 47 | profile.reset(value - target(), duration); 48 | return true; 49 | }; 50 | } 51 | float timeRemaining(float time, Profile &profile) { 52 | return profile.empty() ? 0 : profile.duration() - time; 53 | } 54 | float timeRemaining(void) { 55 | float a = timeRemaining(m_time[0], m_profile[0]); 56 | float b = timeRemaining(m_time[1], m_profile[1]); 57 | return a >= b ? a : b; 58 | } 59 | protected: 60 | Profile m_profile[2]; 61 | float m_time[2]; 62 | float m_offset; 63 | }; 64 | 65 | #endif 66 | -------------------------------------------------------------------------------- /spec/matrix_ext_spec.rb: -------------------------------------------------------------------------------- 1 | require_relative '../matrix_ext' 2 | 3 | 4 | describe Matrix do 5 | it 'should provide the x-vector' do 6 | expect(Matrix.identity(4).x).to eq Vector[1, 0, 0, 0] 7 | end 8 | 9 | it 'should provide the y-vector' do 10 | expect(Matrix.identity(4).y).to eq Vector[0, 1, 0, 0] 11 | end 12 | 13 | it 'should provide the z-vector' do 14 | expect(Matrix.identity(4).z).to eq Vector[0, 0, 1, 0] 15 | end 16 | 17 | it 'should provide the translation vector' do 18 | expect(Matrix.identity(4).translation).to eq Vector[0, 0, 0, 1] 19 | end 20 | 21 | describe :rotate_x do 22 | it 'no rotation should generate identity matrix' do 23 | expect(Matrix.rotate_x(0)).to eq Matrix.identity(4) 24 | end 25 | 26 | it 'should perform rotation around x-axis' do 27 | expect(Matrix.rotate_x(0.5 * Math::PI)).to be_within(1e-6). 28 | of Matrix[[1, 0, 0, 0], [0, 0, -1, 0], [0, 1, 0, 0], [0, 0, 0, 1]] 29 | end 30 | end 31 | 32 | describe :rotate_y do 33 | it 'no rotation should generate identity matrix' do 34 | expect(Matrix.rotate_y(0)).to eq Matrix.identity(4) 35 | end 36 | 37 | it 'should perform rotation around y-axis' do 38 | expect(Matrix.rotate_y(0.5 * Math::PI)).to be_within(1e-6). 39 | of Matrix[[0, 0, 1, 0], [0, 1, 0, 0], [-1, 0, 0, 0], [0, 0, 0, 1]] 40 | end 41 | end 42 | 43 | describe :rotate_z do 44 | it 'no rotation should generate the identity matrix' do 45 | expect(Matrix.rotate_z(0)).to eq Matrix.identity(4) 46 | end 47 | 48 | it 'should perform rotation around z-axis' do 49 | expect(Matrix.rotate_z(0.5 * Math::PI)).to be_within(1e-6). 50 | of Matrix[[0, -1, 0, 0], [1, 0, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]] 51 | end 52 | end 53 | 54 | describe :translate_x do 55 | it 'should perform translation along the x-axis' do 56 | expect(Matrix.translate_x(3)).to eq Matrix[[1, 0, 0, 3], [0, 1, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]] 57 | end 58 | end 59 | 60 | describe :translate_y do 61 | it 'should perform translation along the y-axis' do 62 | expect(Matrix.translate_y(3)).to eq Matrix[[1, 0, 0, 0], [0, 1, 0, 3], [0, 0, 1, 0], [0, 0, 0, 1]] 63 | end 64 | end 65 | 66 | describe :translate_z do 67 | it 'should perform translation along the z-axis' do 68 | expect(Matrix.translate_z(3)).to eq Matrix[[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, 3], [0, 0, 0, 1]] 69 | end 70 | end 71 | 72 | describe :translation do 73 | it 'should perform the specified translation' do 74 | expect(Matrix.translation(2, 3, 5)).to eq Matrix[[1, 0, 0, 2], [0, 1, 0, 3], [0, 0, 1, 5], [0, 0, 0, 1]] 75 | end 76 | end 77 | end 78 | -------------------------------------------------------------------------------- /control.rb: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env ruby 2 | require_relative 'matrix_ext' 3 | require_relative 'joystick' 4 | require_relative 'serial_client' 5 | require_relative 'kinematics' 6 | 7 | 8 | class Control 9 | DEVICE = '/dev/ttyUSB0' 10 | BAUD = 115200 11 | DEADZONE = 4000.0 12 | 13 | attr_reader :position 14 | 15 | def initialize translation_speed = 100, rotation_speed = 1, device = DEVICE, baud = BAUD 16 | @joystick = Joystick.new 17 | @serial_client = SerialClient.new device, baud 18 | @position = Vector[0, 0, 0, 0, 0, 0, 0] 19 | @neutral_pose = Kinematics.forward Vector[0, 0, 0, 0, 0, 0] 20 | @translation_speed = translation_speed 21 | @rotation_speed = rotation_speed 22 | end 23 | 24 | def adapt value 25 | if value > DEADZONE 26 | (value - DEADZONE) / (32768 - DEADZONE) 27 | elsif value < -DEADZONE 28 | (value + DEADZONE) / (32768 - DEADZONE) 29 | else 30 | 0 31 | end 32 | end 33 | 34 | def pose_matrix vector 35 | Matrix.translation(*vector[0 ... 3]) * 36 | Matrix.rotate_y(vector[3]) * 37 | Matrix.rotate_x(vector[4]) * 38 | Matrix.rotate_z(vector[5]) 39 | end 40 | 41 | def degrees vector 42 | vector.collect { |x| x * 180 / Math::PI } 43 | end 44 | 45 | def update elapsed = 1 46 | @joystick.update 47 | axis = @joystick.axis 48 | unless @joystick.button[0] 49 | x = adapt(axis[0] || 0) * @translation_speed 50 | y = adapt(axis[4] || 0) * @translation_speed 51 | z = -adapt(axis[1] || 0) * @translation_speed 52 | b = 0 53 | a = 0 54 | c = adapt(axis[3] || 0) * @rotation_speed 55 | else 56 | x = 0 57 | y = adapt(axis[4] || 0) * @translation_speed 58 | z = 0 59 | a = -adapt(axis[0] || 0) * @rotation_speed 60 | b = adapt(axis[1] || 0) * @rotation_speed 61 | c = adapt(axis[3] || 0) * @rotation_speed 62 | end 63 | gripper = (adapt(axis[5] || 0) - adapt(axis[2] || 0)) * @rotation_speed 64 | offset = Vector[x, y, z, a, b, c, gripper] 65 | @position += offset * elapsed 66 | pose_offset = pose_matrix @position 67 | solution = Kinematics.inverse(@neutral_pose * pose_offset) 68 | unless solution.nil? 69 | target = degrees solution.to_a + [@position[6]] 70 | if @serial_client.ready? and 2 * @serial_client.time_remaining <= @serial_client.time_required(*target) 71 | @serial_client.target *target 72 | end 73 | end 74 | end 75 | 76 | def quit? 77 | @joystick.button[2] || false 78 | end 79 | end 80 | 81 | 82 | if __FILE__ == $0 83 | control = Control.new 84 | time = Time.new.to_f 85 | while not control.quit? 86 | elapsed = Time.new.to_f - time 87 | control.update elapsed 88 | time += elapsed 89 | end 90 | end 91 | -------------------------------------------------------------------------------- /arduino/sainsmart.ino: -------------------------------------------------------------------------------- 1 | #include 2 | #include "../calibration.hh" 3 | #include "../controllerbase.hh" 4 | 5 | 6 | // Humble Object connecting device to tested code http://xunitpatterns.com/Humble%20Object.html 7 | 8 | class Controller: public ControllerBase 9 | { 10 | public: 11 | Controller(void) {} 12 | void setup(void) { 13 | for (int drive=0; drive= 0 76 | roll_angle = Math.atan2 -gripper_vector[1], gripper_vector[2] 77 | else 78 | roll_angle = Math.atan2 gripper_vector[1], -gripper_vector[2] 79 | pitch_angle = -pitch_angle 80 | end 81 | adapter_matrix = Matrix[[0, -1, 0, 0], [0, 0, -1, 0], [1, 0, 0, 0], [0, 0, 0, 0]] 82 | wrist_matrix = Matrix.rotate_x(-pitch_angle) * Matrix.rotate_z(-roll_angle) * adapter_matrix * head_matrix 83 | wrist_vector = wrist_matrix.x 84 | wrist_angle = Math.atan2 wrist_vector[1], wrist_vector[0] 85 | Vector[base_angle, shoulder_angle, elbow_angle - shoulder_angle, roll_angle, pitch_angle, wrist_angle] 86 | else 87 | nil 88 | end 89 | end 90 | end 91 | end 92 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # arduino-sainsmart [![Build Status](https://travis-ci.com/wedesoft/arduino-sainsmart.svg?branch=master)](https://travis-ci.com/wedesoft/arduino-sainsmart) 2 | 3 | 4 | [Arduino][1] software to steer the SainSmart [DIY 6-axis palletizing robot arm][2] and [Sunfounder Rollpaw gripper][15]. 5 | 6 | ![Smooth motion profiles](profile.png) 7 | 8 | The software uses smooth *sin²(t)* (where t is the time) speed profiles to drive the robot joints. 9 | At any time the sum of up to two speed profiles is output to the drives. 10 | Using *sin²(t)+cos²(t)=1* one can achieve constant motion. 11 | The plot shows jerk (blue), acceleration (red), speed (green), and position (magenta). 12 | 13 | [![SainSmart 6-axis servo steering](https://i.ytimg.com/vi/ufObK9-eSjE/hqdefault.jpg)][vid] 14 | 15 | ![Schematics](schematics.jpg) 16 | 17 | ## equipment 18 | 19 | [![SainsSmart 6-axis robot arm](6axis-size.jpg)][2] 20 | 21 | [![Redboard](redboard.jpg)][5] 22 | 23 | [![DFRobot IO expansion shield for Arduino](dfrobot.jpg)][4] 24 | 25 | [![6V DC/3A power supply](power-supply.jpg)][6] 26 | 27 | [![USB Mini-B cable](usb-mini-b.jpg)][7] 28 | 29 | [![2.1 x 5.5mm DC Socket](dc-socket.jpg)][10] 30 | 31 | [![JR Servo Extension Wire](jr-servo-wire.jpg)][17] 32 | 33 | [![Sunfounder Rollpaw gripper](rollpaw.jpg)][15] 34 | 35 | Altogether the equipment cost is about 200£. 36 | Furthermore you need a PC with a USB port. 37 | 38 | ## software build 39 | 40 | First install the dependencies. Please refer to the file *.travis.yml* for more information. 41 | 42 | Create the initial calibration file with the limits and offsets of each servo: 43 | 44 | ``` 45 | cp calibration.hh.default calibration.hh 46 | ``` 47 | 48 | Then build the Arduino program using *make*: 49 | 50 | ``` 51 | make 52 | ``` 53 | 54 | Note: You might have to change the *BOARD_TAG* in the *arduino/Makefile*. 55 | See */usr/share/arduino/hardware/arduino/boards.txt* for supported board tags. 56 | 57 | ## software test 58 | 59 | You can also build and run the tests on the *PC* using the check target: 60 | 61 | ``` 62 | make check 63 | ``` 64 | 65 | ## install on Arduino 66 | 67 | The upload target will upload the program via */dev/ttyUSB0* to the *Arduino* board. 68 | 69 | ``` 70 | make upload 71 | ``` 72 | 73 | **Warning: program the board before connecting the servos the first time to prevent erratic motion!** 74 | 75 | **Warning: once servos are plugged into the board, always connect the servo power to the DFRobot I/O expansion shield before connecting the USB cable to the Arduino to prevent the board power from stalling which causes erratic motion!** 76 | 77 | **Warning: self-collisions or collisions with the surface and other objects can damage the servos!** 78 | 79 | You can then adjust the limits and offsets for your robot and then compile and upload the modified software. 80 | 81 | ## control robot 82 | 83 | You can control the robot using the *screen* serial terminal (make sure *ttyUSB0* is the correct port): 84 | 85 | ``` 86 | screen /dev/ttyUSB0 115200 87 | ``` 88 | 89 | Examples of servo commands are: 90 | 91 | * **o**: check whether drives are ready to receive more commands (1=ready, 0=busy) 92 | * **t**: get time 93 | * **b**: get base servo angle 94 | * **s**: get shoulder servo angle 95 | * **e**: get elbow servo angle 96 | * **r**: get roll servo angle 97 | * **p**: get pitch servo angle 98 | * **w**: get wrist servo angle 99 | * **g**: get gripper servo angle 100 | * **B**: get base servo pulse width 101 | * **S**: get shoulder servo pulse width 102 | * **E**: get elbow servo pulse width 103 | * **R**: get roll servo pulse width 104 | * **P**: get pitch servo pulse width 105 | * **W**: get wrist servo pulse width 106 | * **G**: get gripper servo pulse width 107 | * **c**: get current configuration (base, shoulder, elbow, roll, pitch, and wrist) 108 | * **l**: get lower limits for servos 109 | * **u**: get upper limits for servos 110 | * **45b**: set base servo angle to 45 degrees 111 | * **-12.5s**: set shoulder servo angle to -12.5 degrees 112 | * **10e**: set elbow servo angle to 10 degrees 113 | * **20r**: set roll servo angle to 20 degrees 114 | * **30p**: set pitch servo angle to 30 degrees 115 | * **40w**: set wrist servo angle to 40 degrees 116 | * **0g**: set gripper servo angle to 0 degrees 117 | * **2400B**: set base servo pulse width to 2400 118 | * **1500S**: set shoulder servo pulse width to 1500 119 | * **720E**: set elbow servo pulse width to 720 120 | * **1500R**: set roll servo pulse width to 1500 121 | * **1500P**: set pitch servo pulse width to 1500 122 | * **1500W**: set wrist servo pulse width to 1500 123 | * **2000G**: set gripper servo pulse width to 2000 124 | * **1 2 3 4 5 6c**: set configuration (base, shoulder, elbow, roll, pitch, and wrist) to 1, 2, 3, 4, 5, and 6 degrees 125 | * **1 2 3 4 5 6t**: time required to reach the specified configuration 126 | * **T**: report time required to finish current motion 127 | * **ma**: save teach point *a* (there are 12 teach points from *a* to *l*) 128 | * **'a**: go to teach point *a* 129 | * **da**: display configuration of teach point *a* 130 | * **x**: stop all servos (in fact any undefined key should do) 131 | 132 | You can exit the *screen* terminal using Ctrl-A \\. 133 | 134 | **Warning: self-collisions of the robot can damage the servos!** 135 | 136 | ## XBox controller 137 | 138 | You can control the robot using a calibrated XBox controller. 139 | 140 | ``` 141 | ruby control.rb 142 | ``` 143 | 144 | Note that for some reason you sometimes need to run the serial terminal first, press *t* a few times, and then exit for the Arduino to wake up. 145 | 146 | ![XBox Controller](xbox.png) 147 | 148 | # External links 149 | 150 | * [Sainsmart DIY 6-axis palletizing robot arm][2] (also see [Sainsmart Wiki][11]) 151 | * [Sunfounder Standard Gripper Kit Rollpaw for Robotic Arm][15] ([gripper installation instructions][16]) 152 | * [Redboard][5] ([Arduino][1] compatible board) 153 | * [DFRobot IO expansion shield for Arduino][4] ([manual][18]) 154 | * [6V DC/3A power supply][6] 155 | * [2.1 x 5.5mm DC Socket][10] 156 | * [Sparkfun USB Mini-B cable][7] 157 | * [Towerpro MG996R servo][8] (comes with Sainsmart robot) 158 | * [Towerpro SG90 9g servo][9] (comes with Sainsmart robot; servo shaft not compatible with Sunfounder Rollpaw servos!) 159 | * [22 AWG RC JR Servo Straight Extension Wire 150mm][17] 160 | * [Arduino multitasking part 1][12], [part 2][13], [part 3][14] 161 | * [How to run test headlessly with Xvfb][19] 162 | 163 | [1]: https://www.arduino.cc/ 164 | [2]: https://www.sainsmart.com/products/6-axis-desktop-robotic-arm-assembled 165 | [3]: http://7bot.cc/ 166 | [4]: https://robosavvy.com/store/dfrobot-io-expansion-shield-for-arduino-v6.html 167 | [5]: https://learn.sparkfun.com/tutorials/redboard-vs-uno 168 | [6]: http://uk.rs-online.com/web/p/plug-in-power-supply/7424762/ 169 | [7]: https://robosavvy.com/store/sparkfun-usb-mini-b-cable-6-foot.html 170 | [8]: http://www.hobbyking.com/hobbyking/store/__6221__Towerpro_MG996R_10kg_Servo_10kg_0_20sec_55g.html 171 | [9]: http://www.servodatabase.com/servo/towerpro/sg90 172 | [10]: http://www.maplin.co.uk/p/21-x-55mm-dc-socket-plastic-ft96e 173 | [11]: http://wiki.sainsmart.com/index.php/DIY_6-Axis_Servos_Control_Palletizing_Robot_Arm_Model_for_Arduino_UNO_MEGA2560 174 | [12]: https://learn.adafruit.com/multi-tasking-the-arduino-part-1/ 175 | [13]: https://learn.adafruit.com/multi-tasking-the-arduino-part-2/ 176 | [14]: https://learn.adafruit.com/multi-tasking-the-arduino-part-3/ 177 | [15]: https://www.sunfounder.com/rollpaw.html 178 | [16]: https://www.sunfounder.com/learn/category/Standard-Gripper-Kit-Rollpaw.html 179 | [17]: https://www.amazon.co.uk/d/B00P1716VO 180 | [18]: http://image.dfrobot.com/image/data/Common/Arduino%20Shield%20Manual.pdf 181 | [19]: http://elementalselenium.com/tips/38-headless 182 | [vid]: https://www.youtube.com/watch?v=ufObK9-eSjE 183 | -------------------------------------------------------------------------------- /spec/kinematics_spec.rb: -------------------------------------------------------------------------------- 1 | require_relative '../kinematics' 2 | 3 | 4 | describe Kinematics do 5 | describe :hartenberg do 6 | it 'zero parameters should generate identity matrix' do 7 | expect(Kinematics.hartenberg(0, 0, 0, 0)).to eq Matrix.identity(4) 8 | end 9 | 10 | it 'should perform translations' do 11 | expect(Kinematics.hartenberg(2, 0, 3, 0)). 12 | to eq Matrix[[1, 0, 0, 3], [0, 1, 0, 0], [0, 0, 1, 2], [0, 0, 0, 1]] 13 | end 14 | 15 | it 'should rotate around z-axis' do 16 | expect(Kinematics.hartenberg(2, 0.5 * Math::PI, 3, 0)).to be_within(1e-6). 17 | of Matrix[[0, -1, 0, 0], [1, 0, 0, 3], [0, 0, 1, 2], [0, 0, 0, 1]] 18 | end 19 | 20 | it 'should rotate around x-axis' do 21 | expect(Kinematics.hartenberg(2, 0, 3, 0.5 * Math::PI)).to be_within(1e-6). 22 | of Matrix[[1, 0, 0, 3], [0, 0, -1, 0], [0, 1, 0, 2], [0, 0, 0, 1]] 23 | end 24 | end 25 | 26 | BASE = Kinematics::BASE 27 | FOOT = Kinematics::FOOT 28 | SHOULDER = Kinematics::SHOULDER 29 | KNEE = Kinematics::KNEE 30 | ELBOW = Kinematics::ELBOW 31 | GRIPPER = Kinematics::GRIPPER 32 | 33 | let(:pi2) { Math::PI / 2 } 34 | let(:pi4) { Math::PI / 4 } 35 | let(:origin) { Vector[0, 0, 0, 1] } 36 | let(:x) { Vector[1, 0, 0, 0] } 37 | let(:y) { Vector[0, 1, 0, 0] } 38 | let(:z) { Vector[0, 0, 1, 0] } 39 | 40 | describe :base do 41 | it 'should have the correct center' do 42 | expect(Kinematics.base(0) * origin).to eq Vector[FOOT, 0, BASE, 1] 43 | end 44 | 45 | it 'should orient the shoulder axis' do 46 | expect(Kinematics.base(0) * z).to be_within(1e-6).of -y 47 | end 48 | 49 | it 'should have the correct x-axis' do 50 | expect(Kinematics.base(0) * x).to be_within(1e-6).of x 51 | end 52 | 53 | it 'should support rotation of the base' do 54 | expect(Kinematics.base(pi2) * origin).to be_within(1e-6). 55 | of Vector[0, FOOT, BASE, 1] 56 | end 57 | end 58 | 59 | describe :shoulder do 60 | it 'should orient the z-axis' do 61 | expect(Kinematics.shoulder(0, -pi2) * z).to be_within(1e-6).of y 62 | end 63 | 64 | it 'should have the correct x-axis' do 65 | expect(Kinematics.shoulder(0, -pi2) * x).to be_within(1e-6).of x 66 | end 67 | 68 | it 'should have the correct center' do 69 | expect(Kinematics.shoulder(0, 0) * origin).to be_within(1e-6). 70 | of Vector[FOOT, 0, BASE + SHOULDER, 1] 71 | end 72 | 73 | it 'should support rotation' do 74 | expect(Kinematics.shoulder(0, -pi2) * origin).to be_within(1e-6). 75 | of Vector[FOOT + SHOULDER, 0, BASE, 1] 76 | end 77 | end 78 | 79 | describe :elbow do 80 | it 'should orient the z-axis' do 81 | expect(Kinematics.elbow(0, 0, 0) * z).to be_within(1e-6).of x 82 | end 83 | 84 | it 'should orient the x-axis' do 85 | expect(Kinematics.elbow(0, 0, 0) * x).to be_within(1e-6).of -z 86 | end 87 | 88 | it 'should have the correct center' do 89 | expect(Kinematics.elbow(0, 0, 0) * origin).to be_within(1e-6). 90 | of Vector[FOOT, 0, BASE + SHOULDER + KNEE, 1] 91 | end 92 | 93 | it 'should support rotation' do 94 | expect(Kinematics.elbow(0, 0, pi2) * origin).to be_within(1e-6). 95 | of Vector[FOOT + KNEE, 0, BASE + SHOULDER, 1] 96 | end 97 | end 98 | 99 | describe :roll do 100 | it 'should orient the z-axis' do 101 | expect(Kinematics.roll(0, 0, 0, 0) * z).to be_within(1e-6).of -y 102 | end 103 | 104 | it 'should orient the x-axis' do 105 | expect(Kinematics.roll(0, 0, 0, 0) * x).to be_within(1e-6).of -z 106 | end 107 | 108 | it 'should have the correct center' do 109 | expect(Kinematics.roll(0, 0, 0, 0) * origin).to be_within(1e-6). 110 | of Vector[FOOT + ELBOW, 0, BASE + SHOULDER + KNEE, 1] 111 | end 112 | 113 | it 'should support rotation' do 114 | expect(Kinematics.roll(0, 0, 0, pi2) * x).to be_within(1e-6).of y 115 | end 116 | end 117 | 118 | describe :pitch do 119 | it 'should orient the z-axis' do 120 | expect(Kinematics.pitch(0, 0, 0, 0, 0) * z).to be_within(1e-6).of x 121 | end 122 | 123 | it 'should orient the x-axis' do 124 | expect(Kinematics.pitch(0, 0, 0, 0, 0) * x).to be_within(1e-6).of z 125 | end 126 | 127 | it 'should have the correct center' do 128 | expect(Kinematics.pitch(0, 0, 0, 0, 0) * origin).to be_within(1e-6). 129 | of Vector[FOOT + ELBOW, 0, BASE + SHOULDER + KNEE, 1] 130 | end 131 | 132 | it 'should support rotation' do 133 | expect(Kinematics.pitch(0, 0, 0, 0, pi2) * z).to be_within(1e-6).of z 134 | end 135 | end 136 | 137 | describe :wrist do 138 | it 'should orient the z-axis' do 139 | expect(Kinematics.wrist(0, 0, 0, 0, 0, 0) * z).to be_within(1e-6).of x 140 | end 141 | 142 | it 'should orient the x-axis along the way the gripper opens' do 143 | expect(Kinematics.wrist(0, 0, 0, 0, 0, 0) * x).to be_within(1e-6).of -y 144 | end 145 | 146 | it 'should have the correct center' do 147 | expect(Kinematics.wrist(0, 0, 0, 0, 0, 0) * origin).to be_within(1e-6). 148 | of Vector[FOOT + ELBOW + GRIPPER, 0, BASE + SHOULDER + KNEE, 1] 149 | end 150 | 151 | it 'should support rotation' do 152 | expect(Kinematics.wrist(0, 0, 0, 0, 0, pi2) * x).to be_within(1e-6).of -z 153 | end 154 | end 155 | 156 | describe :forward do 157 | it 'should invoke the complete kinematic chain' do 158 | expect(Kinematics).to receive(:wrist).with 2, 3, 8, 7, 11, 13 159 | Kinematics.forward Vector[2, 3, 5, 7, 11, 13] 160 | end 161 | end 162 | 163 | describe :inverse_kinematics do 164 | def round_trip *args 165 | Kinematics.inverse Kinematics.forward(*args) 166 | end 167 | 168 | it 'should work for the neutral position' do 169 | expect(round_trip(Vector[0, 0, 0, 0, 0, 0])).to be_within(1e-6).of Vector[0, 0, 0, 0, 0, 0] 170 | end 171 | 172 | it 'should determine rotation of base joint' do 173 | expect(round_trip(Vector[pi2, 0, 0, 0, 0, 0])).to be_within(1e-6).of Vector[pi2, 0, 0, 0, 0, 0] 174 | end 175 | 176 | it 'should determine the base joint angle when the gripper is orthogonal to the elbow' do 177 | expect(round_trip(Vector[pi2, 0, 0, pi2, pi2, 0])[0]).to be_within(1e-6).of pi2 178 | end 179 | 180 | it 'should determine the base joint when the gripper is tilted' do 181 | expect(round_trip(Vector[0, 0, 0, pi2, pi4, 0])[0]).to be_within(1e-6).of 0 182 | end 183 | 184 | it 'should determine the shoulder angle' do 185 | expect(round_trip(Vector[0, -pi2, pi2, 0, 0, 0])).to be_within(1e-6).of Vector[0, -pi2, pi2, 0, 0, 0] 186 | end 187 | 188 | it 'should determine the elbow angle' do 189 | expect(round_trip(Vector[0, 0, pi4, 0, 0, 0])).to be_within(1e-6).of Vector[0, 0, pi4, 0, 0, 0] 190 | end 191 | 192 | it 'should determine the roll angle' do 193 | expect(round_trip(Vector[0, 0, 0, pi4, pi2, 0])[3]).to be_within(1e-6).of pi4 194 | end 195 | 196 | it 'should restrict the roll angle' do 197 | expect(round_trip(Vector[0, 0, 0, pi4, -pi2, 0])[3]).to be_within(1e-6).of pi4 198 | end 199 | 200 | it 'should determine the pitch angle' do 201 | expect(round_trip(Vector[0, 0, 0, pi4, pi2, 0])[4]).to be_within(1e-6).of pi2 202 | end 203 | 204 | it 'should support negative pitch angles' do 205 | expect(round_trip(Vector[0, 0, 0, pi4, -pi2, 0])[4]).to be_within(1e-6).of -pi2 206 | end 207 | 208 | it 'should determine the wrist angle' do 209 | expect(round_trip(Vector[0.6, 0.5, 0.4, 0.3, 0.2, 0.1])[5]).to be_within(1e-6).of 0.1 210 | end 211 | 212 | it 'should return nil if there is no solution' do 213 | expect(Kinematics.inverse(Matrix.translation(0, 0, 1000))).to be nil 214 | end 215 | end 216 | end 217 | -------------------------------------------------------------------------------- /spec/control_spec.rb: -------------------------------------------------------------------------------- 1 | require_relative '../control' 2 | 3 | 4 | describe Control do 5 | let(:joystick) { double 'joystick' } 6 | let(:neutral_pose) { double 'neutral_pose' } 7 | let(:serial_client) { double 'serial_client' } 8 | let(:pose) { double 'pose' } 9 | let(:target_radians) { double 'target_radians' } 10 | let(:target_degrees) { double 'target_degrees' } 11 | let(:control) { Control.new 1, 1 } 12 | 13 | before :each do 14 | allow(Joystick).to receive(:new).and_return joystick 15 | allow(joystick).to receive :update 16 | allow(joystick).to receive(:axis).and_return({}) 17 | allow(joystick).to receive(:button).and_return({}) 18 | allow(SerialClient).to receive(:new).and_return serial_client 19 | allow(serial_client).to receive :target 20 | allow(serial_client).to receive(:ready?).and_return true 21 | allow(serial_client).to receive(:time_remaining).and_return 5 22 | allow(serial_client).to receive(:time_required).and_return 11 23 | allow(neutral_pose).to receive(:*).and_return pose 24 | allow(Kinematics).to receive(:forward).and_return neutral_pose 25 | allow(Kinematics).to receive(:inverse).and_return(Vector[target_radians]) 26 | end 27 | 28 | describe :initialize do 29 | it 'should initialize the joystick' do 30 | expect(Joystick).to receive :new 31 | Control.new 32 | end 33 | 34 | it 'should initialize serial communication' do 35 | expect(SerialClient).to receive(:new).with '/dev/ttyUSB0', 115200 36 | Control.new 1, 1, '/dev/ttyUSB0', 115200 37 | end 38 | 39 | it 'should start with a zero position offset' do 40 | expect(Control.new.position).to eq Vector[0, 0, 0, 0, 0, 0, 0] 41 | end 42 | 43 | it 'should determine the forward kinematics of the neutral configuration' do 44 | expect(Kinematics).to receive(:forward).with Vector[0, 0, 0, 0, 0, 0] 45 | Control.new 46 | end 47 | end 48 | 49 | describe :adapt do 50 | it 'should map zero to zero' do 51 | expect(control.adapt(0)).to eq 0 52 | end 53 | 54 | it 'should map 32768 to one' do 55 | expect(control.adapt(32768)).to eq 1 56 | end 57 | 58 | it 'should use a dead zone' do 59 | expect(control.adapt(Control::DEADZONE)).to eq 0 60 | end 61 | 62 | it 'should scale linearly above dead zone' do 63 | expect(control.adapt(2 * Control::DEADZONE)).to be_between(0, 1).exclusive 64 | end 65 | 66 | it 'should map -32768 to one' do 67 | expect(control.adapt(-32768)).to eq -1 68 | end 69 | 70 | it 'should use a negative dead zone' do 71 | expect(control.adapt(-Control::DEADZONE)).to eq 0 72 | end 73 | 74 | it 'should scale linearly below the negative dead zone' do 75 | expect(control.adapt(-2 * Control::DEADZONE)).to be_between(-1, 0).exclusive 76 | end 77 | end 78 | 79 | describe :pose_matrix do 80 | it 'should create a translation matrix' do 81 | expect(control.pose_matrix(Vector[2, 3, 5, 0, 0, 0, 0])). 82 | to eq Matrix[[1, 0, 0, 2], [0, 1, 0, 3], [0, 0, 1, 5], [0, 0, 0, 1]] 83 | end 84 | 85 | it 'should perform rotations' do 86 | expect(control.pose_matrix(Vector[2, 3, 5, 0.1, 0.2, 0.3, 0])). 87 | to be_within(1e-6).of Matrix.translation(2, 3, 5) * Matrix.rotate_y(0.1) * Matrix.rotate_x(0.2) * Matrix.rotate_z(0.3) 88 | end 89 | end 90 | 91 | describe :degrees do 92 | it 'should convert angles from radians to degrees' do 93 | expect(control.degrees([0, Math::PI, 2 * Math::PI])).to eq [0, 180, 360] 94 | end 95 | end 96 | 97 | describe :update do 98 | let(:pose_offset) { double 'pose_offset' } 99 | before :each do 100 | allow(control).to receive(:adapt).and_return 0.2, 0.5, 1.0, 1.2 101 | allow(control).to receive(:pose_matrix).and_return pose_offset 102 | allow(control).to receive(:degrees).and_return target_degrees 103 | end 104 | 105 | it 'should update the joystick' do 106 | expect(joystick).to receive :update 107 | control.update 108 | end 109 | 110 | it 'should convert positional changes' do 111 | expect(joystick).to receive(:axis).and_return({0 => 2, 4 => 3, 1 => 5, 3 => 7}) 112 | expect(control).to receive(:adapt).with(2).ordered.and_return 0.2 113 | expect(control).to receive(:adapt).with(3).ordered.and_return 0.5 114 | expect(control).to receive(:adapt).with(5).ordered.and_return 1.0 115 | expect(control).to receive(:adapt).with(7).ordered.and_return 1.2 116 | control.update 117 | end 118 | 119 | it 'should apply the positional offset' do 120 | expect(control).to receive(:adapt).and_return 0.2, 0.5, 1.0, 1.2, 0, 0 121 | control.update 122 | expect(control.position).to eq Vector[0.2, 0.5, -1.0, 0, 0, 1.2, 0] 123 | end 124 | 125 | it 'should accumulate the positional offset' do 126 | expect(control).to receive(:adapt).and_return 0.2, 0.5, 1.0, 1.2, 0, 0, 0.2, 0.5, 1.0, 1.2, 0, 0 127 | control.update 128 | control.update 129 | expect(control.position).to eq Vector[0.4, 1.0, -2.0, 0, 0, 2.4, 0] 130 | end 131 | 132 | it 'should use the specified speed values' do 133 | control = Control.new 2, 4 134 | allow(control).to receive(:degrees).and_return target_degrees 135 | expect(control).to receive(:adapt).and_return 0.2, 0.5, 1.0, 1.2, 0, 0 136 | control.update 137 | expect(control.position).to eq Vector[0.4, 1.0, -2.0, 0, 0, 4.8, 0] 138 | end 139 | 140 | it 'should use the specified time step' do 141 | expect(control).to receive(:adapt).and_return 0.2, 0.5, 1.0, 1.2, 0, 0 142 | control.update 2 143 | expect(control.position).to eq Vector[0.4, 1.0, -2.0, 0, 0, 2.4, 0] 144 | end 145 | 146 | it 'should determine the pose matrix' do 147 | expect(control).to receive(:pose_matrix).with Vector[0.2, 0.5, -1.0, 0, 0, 1.2, 0] 148 | control.update 149 | end 150 | 151 | it 'should invoke the inverse kinematics' do 152 | expect(Kinematics).to receive(:inverse).with pose 153 | control.update 154 | end 155 | 156 | it 'should convert the angles to degrees' do 157 | expect(control).to receive(:degrees).with([target_radians, 0]) 158 | control.update 159 | end 160 | 161 | it 'should target the dessired configuration' do 162 | expect(serial_client).to receive(:target).with *target_degrees 163 | control.update 164 | end 165 | 166 | it 'should only target the desired configuration if the microcontroller is ready' do 167 | expect(serial_client).to receive(:ready?).and_return false 168 | expect(serial_client).to_not receive :target 169 | control.update 170 | end 171 | 172 | it 'should only submit a target if it requires more than twice the remaining time of the current target' do 173 | expect(serial_client).to receive(:time_remaining).and_return 5 174 | expect(serial_client).to receive(:time_required).with(*target_degrees).and_return 9 175 | expect(serial_client).to_not receive :target 176 | control.update 177 | end 178 | 179 | it 'should not submit a target if the inverse kinematics does not return a solution' do 180 | allow(Kinematics).to receive(:inverse).and_return nil 181 | expect(serial_client).to_not receive :target 182 | control.update 183 | end 184 | 185 | context 'when the A button is pressed' do 186 | before :each do 187 | allow(joystick).to receive(:button).and_return({0 => true}) 188 | end 189 | 190 | it 'should convert rotational changes' do 191 | expect(joystick).to receive(:axis).and_return({4 => 2, 0 => 3, 1 => 5, 3 => 7, 5 => 11, 2 => 13}) 192 | expect(control).to receive(:adapt).with(2).ordered.and_return 0.2 193 | expect(control).to receive(:adapt).with(3).ordered.and_return 0.5 194 | expect(control).to receive(:adapt).with(5).ordered.and_return 1.0 195 | expect(control).to receive(:adapt).with(7).ordered.and_return 1.2 196 | expect(control).to receive(:adapt).with(11).ordered.and_return 0 197 | expect(control).to receive(:adapt).with(13).ordered.and_return 0 198 | control.update 199 | end 200 | 201 | it 'should apply the rotational offset' do 202 | expect(control).to receive(:adapt).and_return 0.2, 0.5, 1.0, 1.2, 0, 0 203 | control.update 204 | expect(control.position).to eq Vector[0, 0.2, 0, -0.5, 1.0, 1.2, 0] 205 | end 206 | 207 | it 'should use the specified speed values' do 208 | control = Control.new 2, 4 209 | allow(control).to receive(:degrees).and_return target_degrees 210 | expect(control).to receive(:adapt).and_return 0.2, 0.5, 1.0, 1.2, 0, 0 211 | control.update 212 | expect(control.position).to eq Vector[0, 0.4, 0, -2.0, 4.0, 4.8, 0] 213 | end 214 | 215 | end 216 | 217 | it 'should update the gripper' do 218 | allow(joystick).to receive(:axis).and_return({5 => 2}) 219 | allow(control).to receive(:adapt).and_return 0 220 | expect(control).to receive(:adapt).and_return 0, 0, 0, 0, 2, 0 221 | control.update 222 | expect(control.position).to eq Vector[0, 0, 0, 0, 0, 0, 2] 223 | end 224 | 225 | it 'should use the difference of two axes for the gripper' do 226 | allow(joystick).to receive(:axis).and_return({5 => 2, 2 => 2}) 227 | expect(control).to receive(:adapt).and_return 0, 0, 0, 0, 2, 2 228 | control.update 229 | expect(control.position).to eq Vector[0, 0, 0, 0, 0, 0, 0] 230 | end 231 | 232 | it 'should convert the gripper angle to degrees' do 233 | allow(joystick).to receive(:axis).and_return({5 => 2}) 234 | expect(control).to receive(:adapt).and_return 0, 0, 0, 0, 2, 0 235 | expect(control).to receive(:degrees).with([target_radians, 2]) 236 | control.update 237 | end 238 | end 239 | 240 | describe :quit? do 241 | it 'should return false by default' do 242 | expect(control.quit?).to be false 243 | end 244 | 245 | it 'should return true if the X button is pressed' do 246 | expect(joystick).to receive(:button).and_return({2 => true}) 247 | expect(control.quit?).to be true 248 | end 249 | end 250 | end 251 | -------------------------------------------------------------------------------- /controllerbase.hh: -------------------------------------------------------------------------------- 1 | #ifndef __CONTROLLER_HH 2 | #define __CONTROLLER_HH 3 | 4 | #include "calibration.hh" 5 | #include "path.hh" 6 | 7 | const int BASE = 0; 8 | const int SHOULDER = 1; 9 | const int ELBOW = 2; 10 | const int ROLL = 3; 11 | const int PITCH = 4; 12 | const int WRIST = 5; 13 | const int GRIPPER = 6; 14 | const int DRIVES = 7; 15 | 16 | const float ELBOW_RANGE = 60; 17 | 18 | class ControllerBase 19 | { 20 | public: 21 | ControllerBase(void): m_number(0), m_fraction(0), m_sign(0), m_teachFun(NULL), m_index(0) { 22 | memset(m_teach, 0, sizeof(m_teach)); 23 | memset(m_configuration, 0, sizeof(m_configuration)); 24 | } 25 | 26 | virtual ~ControllerBase() {} 27 | 28 | Path &curve(int drive) { return m_curve[drive]; } 29 | 30 | int drive(char c) { 31 | switch (tolower(c)) { 32 | case 's': 33 | return SHOULDER; 34 | case 'e': 35 | return ELBOW; 36 | case 'r': 37 | return ROLL; 38 | case 'p': 39 | return PITCH; 40 | case 'w': 41 | return WRIST; 42 | case 'g': 43 | return GRIPPER; 44 | default: 45 | return BASE; 46 | }; 47 | } 48 | 49 | float target(int drive) { 50 | return m_curve[drive].target(); 51 | } 52 | 53 | float limit(float value, float lower, float upper) { 54 | return value < lower ? lower : value > upper ? upper : value; 55 | } 56 | 57 | float angleToPWM(int drive, float angle) { 58 | return offset(drive) + angle * resolution(drive); 59 | } 60 | 61 | float pwmToAngle(int drive, float pwm) { 62 | return (pwm - offset(drive)) / resolution(drive); 63 | } 64 | 65 | float clipPWM(int drive, float value) { 66 | return limit(value, lower(drive), upper(drive)); 67 | } 68 | 69 | float clipAngle(int drive, float value) { 70 | return pwmToAngle(drive, clipPWM(drive, angleToPWM(drive, value))); 71 | } 72 | 73 | float limitJoint(float value, float other) { 74 | return limit(value, -ELBOW_RANGE - other, ELBOW_RANGE - other); 75 | } 76 | 77 | float limitArmAngle(int drive, float value) 78 | { 79 | switch (drive) { 80 | case ELBOW: 81 | return limitJoint(value, target(SHOULDER)); 82 | case SHOULDER: 83 | return limitJoint(value, target(ELBOW)); 84 | default: 85 | return value; 86 | }; 87 | } 88 | 89 | void saveTeachPoint(int index) { 90 | for (int i=0; i= 'a' && c <= 'l') 189 | (this->*m_teachFun)(c - 'a'); 190 | else 191 | stopDrives(); 192 | resetParser(); 193 | m_teachFun = NULL; 194 | } else { 195 | switch (c) { 196 | case 'o': 197 | reportReady(drivesReady()); 198 | resetParser(); 199 | break; 200 | case 't': 201 | if (hasNumber()) { 202 | takeConfigurationValue(); 203 | reportRequired(timeRequired(m_configuration)); 204 | } else 205 | reportTime(); 206 | resetParser(); 207 | break; 208 | case 'T': 209 | reportRemaining(m_curve[BASE].timeRemaining()); 210 | resetParser(); 211 | break; 212 | case '.': 213 | if (m_fraction > 0) 214 | resetParser(); 215 | else 216 | m_fraction = 1; 217 | break; 218 | case '-': 219 | m_sign = m_sign == 0 ? -1 : -m_sign; 220 | m_number = 0; 221 | m_fraction = 0; 222 | break; 223 | case 'b': 224 | case 'B': 225 | case 'e': 226 | case 'E': 227 | case 's': 228 | case 'S': 229 | case 'r': 230 | case 'R': 231 | case 'p': 232 | case 'P': 233 | case 'w': 234 | case 'W': 235 | case 'g': 236 | case 'G': 237 | if (hasNumber()) { 238 | if (isupper(c)) 239 | targetPWM(drive(c), number()); 240 | else 241 | targetAngle(drive(c), number()); 242 | } else 243 | if (isupper(c)) 244 | reportPWM(round(angleToPWM(drive(c), m_curve[drive(c)].pos()))); 245 | else 246 | reportAngle(m_curve[drive(c)].pos()); 247 | resetParser(); 248 | break; 249 | case '0': 250 | case '1': 251 | case '2': 252 | case '3': 253 | case '4': 254 | case '5': 255 | case '6': 256 | case '7': 257 | case '8': 258 | case '9': 259 | m_number = 10 * m_number + (c - '0'); 260 | m_fraction *= 0.1; 261 | if (m_sign == 0) m_sign = 1; 262 | break; 263 | case '\'': 264 | m_teachFun = &ControllerBase::loadTeachPoint; 265 | break; 266 | case 'm': 267 | m_teachFun = &ControllerBase::saveTeachPoint; 268 | break; 269 | case 'd': 270 | m_teachFun = &ControllerBase::displayTeachPoint; 271 | break; 272 | case ' ': 273 | takeConfigurationValue(); 274 | break; 275 | case 'c': 276 | if (hasNumber()) { 277 | takeConfigurationValue(); 278 | targetPoint(m_configuration); 279 | } else 280 | reportConfiguration(m_curve[0].pos(), m_curve[1].pos(), m_curve[2].pos(), 281 | m_curve[3].pos(), m_curve[4].pos(), m_curve[5].pos(), 282 | m_curve[6].pos()); 283 | resetParser(); 284 | break; 285 | case 'l': 286 | reportLower((lower(0) - offset(0)) / resolution(0), 287 | (lower(1) - offset(1)) / resolution(1), 288 | (lower(2) - offset(2)) / resolution(2), 289 | (lower(3) - offset(3)) / resolution(3), 290 | (lower(4) - offset(4)) / resolution(4), 291 | (lower(5) - offset(5)) / resolution(5), 292 | (lower(6) - offset(6)) / resolution(6)); 293 | resetParser(); 294 | break; 295 | case 'u': 296 | reportUpper((upper(0) - offset(0)) / resolution(0), 297 | (upper(1) - offset(1)) / resolution(1), 298 | (upper(2) - offset(2)) / resolution(2), 299 | (upper(3) - offset(3)) / resolution(3), 300 | (upper(4) - offset(4)) / resolution(4), 301 | (upper(5) - offset(5)) / resolution(5), 302 | (upper(6) - offset(6)) / resolution(6)); 303 | resetParser(); 304 | break; 305 | default: 306 | stopDrives(); 307 | }; 308 | }; 309 | } 310 | 311 | virtual int offset(int drive) = 0; 312 | virtual float resolution(int drive) = 0; 313 | virtual int lower(int drive) = 0; 314 | virtual int upper(int drive) = 0; 315 | virtual void reportReady(bool ready) = 0; 316 | virtual void reportTime(void) = 0; 317 | virtual void reportRequired(float time) = 0; 318 | virtual void reportRemaining(float time) = 0; 319 | virtual void reportAngle(float) = 0; 320 | virtual void reportPWM(int) = 0; 321 | virtual void reportConfiguration(float, float, float, float, float, float, float) = 0; 322 | virtual void reportLower(float, float, float, float, float, float, float) = 0; 323 | virtual void reportUpper(float, float, float, float, float, float, float) = 0; 324 | virtual void reportTeachPoint(float, float, float, float, float, float, float) = 0; 325 | virtual void writePWM(int, int) = 0; 326 | 327 | protected: 328 | float m_number; 329 | float m_fraction; 330 | char m_sign; 331 | void (ControllerBase::*m_teachFun)(int); 332 | float m_teach[12][DRIVES]; 333 | int m_index; 334 | float m_configuration[DRIVES]; 335 | Path m_curve[DRIVES]; 336 | }; 337 | 338 | #endif 339 | -------------------------------------------------------------------------------- /test-suite.cc: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include "controllerbase.hh" 4 | #include "profile.hh" 5 | #include "path.hh" 6 | 7 | using namespace testing; 8 | 9 | TEST(ProfileTest, DefaultProfile) 10 | { 11 | EXPECT_EQ(0, Profile().value(-1)); 12 | EXPECT_EQ(0, Profile().value( 0)); 13 | EXPECT_EQ(0, Profile().value( 1)); 14 | } 15 | 16 | TEST(ProfileTest, StartWithZero) 17 | { 18 | EXPECT_NEAR(0, Profile(1, 10).value(0.001), 0.01); 19 | } 20 | 21 | TEST(ProfileTest, EmptyOrNot) 22 | { 23 | ASSERT_TRUE(Profile().empty()); 24 | ASSERT_FALSE(Profile(1, 10).empty()); 25 | } 26 | 27 | TEST(ProfileTest, EndWithDistance) 28 | { 29 | EXPECT_NEAR(1, Profile(1, 10).value(9.999), 0.001); 30 | EXPECT_NEAR(2, Profile(2, 10).value(9.999), 0.001); 31 | } 32 | 33 | TEST(ProfileTest, PassesMiddle) 34 | { 35 | EXPECT_NEAR(0.5, Profile(1, 10).value(5), 0.001); 36 | } 37 | 38 | TEST(ProfileTest, DistanceCubicWithDuration) 39 | { 40 | EXPECT_NEAR(2 * Profile::timeRequired(1, 0.01), Profile::timeRequired(8, 0.01), 0.001); 41 | } 42 | 43 | TEST(ProfileTest, DistanceCubicWithMaxJerk) 44 | { 45 | EXPECT_NEAR(2 * Profile::timeRequired(1, 0.08), Profile::timeRequired(1, 0.01), 0.001); 46 | } 47 | 48 | TEST(ProfileTest, TestTimeRequiredPoint) 49 | { 50 | EXPECT_NEAR(2, Profile::timeRequired(200, 314), 2.001); 51 | } 52 | 53 | TEST(ProfileTest, Accelerates) 54 | { 55 | EXPECT_GT(0.2, Profile(1, 4).value(1)); 56 | } 57 | 58 | TEST(ProfileTest, StartAndEndStationary) 59 | { 60 | EXPECT_FLOAT_EQ( 0, Profile(123, 10).value(-1)); 61 | EXPECT_FLOAT_EQ(123, Profile(123, 10).value(11)); 62 | } 63 | 64 | class PathTest: public Test { 65 | protected: 66 | Path m_path; 67 | }; 68 | 69 | TEST_F(PathTest, StartWithZero) 70 | { 71 | EXPECT_EQ(0, m_path.pos()); 72 | } 73 | 74 | TEST_F(PathTest, StayConstantOverTime) 75 | { 76 | m_path.update(5); 77 | EXPECT_EQ(0, m_path.pos()); 78 | } 79 | 80 | TEST_F(PathTest, StopSetsValue) 81 | { 82 | m_path.stop(100); 83 | EXPECT_EQ(100, m_path.pos()); 84 | } 85 | 86 | TEST_F(PathTest, ApproachTarget) 87 | { 88 | m_path.stop(100); 89 | m_path.retarget(200, 1); 90 | m_path.update(0.5); 91 | EXPECT_NEAR(150, m_path.pos(), 0.001); 92 | m_path.update(0.5); 93 | EXPECT_NEAR(200, m_path.pos(), 0.001); 94 | } 95 | 96 | TEST_F(PathTest, UseSpecifiedDuration) 97 | { 98 | m_path.stop(100); 99 | m_path.retarget(200, 2); 100 | m_path.update(1); 101 | EXPECT_NEAR(150, m_path.pos(), 0.001); 102 | m_path.update(1); 103 | EXPECT_NEAR(200, m_path.pos(), 0.001); 104 | } 105 | 106 | TEST_F(PathTest, UpdateReturnsPosition) 107 | { 108 | m_path.stop(100); 109 | m_path.retarget(200, 2); 110 | EXPECT_NEAR(150, m_path.update(1), 0.001); 111 | EXPECT_NEAR(200, m_path.update(1), 0.001); 112 | } 113 | 114 | TEST_F(PathTest, AbortMotion) { 115 | m_path.stop(100); 116 | m_path.retarget(200, 2); 117 | m_path.update(1); 118 | m_path.stop(m_path.pos()); 119 | m_path.update(1); 120 | EXPECT_NEAR(150, m_path.pos(), 0.001); 121 | } 122 | 123 | TEST_F(PathTest, ModifyTarget) { 124 | m_path.stop(100); 125 | m_path.retarget(200, 2); 126 | m_path.update(1); 127 | m_path.retarget(300, 2); 128 | EXPECT_NEAR(150, m_path.pos(), 0.001); 129 | m_path.update(1); 130 | EXPECT_NEAR(250, m_path.pos(), 0.001); 131 | m_path.update(1); 132 | EXPECT_NEAR(300, m_path.pos(), 0.001); 133 | } 134 | 135 | TEST_F(PathTest, IgnoreThirdTarget) { 136 | m_path.retarget(1, 1); 137 | m_path.retarget(2, 1); 138 | m_path.retarget(4, 1); 139 | m_path.update(1); 140 | EXPECT_FLOAT_EQ(2, m_path.pos()); 141 | } 142 | 143 | TEST_F(PathTest, MakeFirstProfileAvailableWhenFinished) { 144 | m_path.retarget(1, 1); 145 | m_path.retarget(2, 2); 146 | m_path.update(1); 147 | m_path.retarget(4, 1); 148 | m_path.update(1); 149 | EXPECT_FLOAT_EQ(4, m_path.pos()); 150 | } 151 | 152 | TEST_F(PathTest, MakeSecondProfileAvailableWhenFinished) { 153 | m_path.retarget(1, 2); 154 | m_path.retarget(2, 1); 155 | m_path.update(1); 156 | m_path.retarget(4, 1); 157 | m_path.update(1); 158 | EXPECT_FLOAT_EQ(4, m_path.pos()); 159 | } 160 | 161 | TEST_F(PathTest, QueryTarget) { 162 | m_path.stop(100); 163 | EXPECT_FLOAT_EQ(100, m_path.target()); 164 | m_path.retarget(200, 2); 165 | m_path.update(1); 166 | EXPECT_FLOAT_EQ(200, m_path.target()); 167 | m_path.retarget(300, 2); 168 | m_path.update(1); 169 | EXPECT_FLOAT_EQ(300, m_path.target()); 170 | } 171 | 172 | TEST_F(PathTest, QueryTimeRequired) { 173 | m_path.stop(100); 174 | EXPECT_FLOAT_EQ(0, m_path.timeRemaining()); 175 | m_path.retarget(200, 3); 176 | EXPECT_FLOAT_EQ(3, m_path.timeRemaining()); 177 | m_path.update(1); 178 | EXPECT_FLOAT_EQ(2, m_path.timeRemaining()); 179 | m_path.retarget(300, 3); 180 | EXPECT_FLOAT_EQ(3, m_path.timeRemaining()); 181 | m_path.update(4); 182 | EXPECT_FLOAT_EQ(0, m_path.timeRemaining()); 183 | } 184 | 185 | TEST_F(PathTest, ReadyCheck) { 186 | EXPECT_TRUE(m_path.ready()); 187 | m_path.retarget(100, 1); 188 | EXPECT_TRUE(m_path.ready()); 189 | m_path.retarget(200, 3); 190 | EXPECT_FALSE(m_path.ready()); 191 | m_path.update(2); 192 | EXPECT_TRUE(m_path.ready()); 193 | } 194 | 195 | class MockController: public ControllerBase 196 | { 197 | public: 198 | int offset(int drive) { return 1500; } 199 | float resolution(int drive) { return 12.0; } 200 | int lower(int drive) { return 544; } 201 | int upper(int drive) { return 2400; } 202 | MOCK_METHOD1(reportReady, void(bool)); 203 | MOCK_METHOD0(reportTime, void()); 204 | MOCK_METHOD1(reportRequired, void(float)); 205 | MOCK_METHOD1(reportRemaining, void(float)); 206 | MOCK_METHOD1(reportAngle, void(float)); 207 | MOCK_METHOD1(reportPWM, void(int)); 208 | MOCK_METHOD2(writePWM, void(int,int)); 209 | MOCK_METHOD7(reportConfiguration, void(float,float,float,float,float,float, float)); 210 | MOCK_METHOD7(reportLower, void(float,float,float,float,float,float, float)); 211 | MOCK_METHOD7(reportUpper, void(float,float,float,float,float,float, float)); 212 | MOCK_METHOD7(reportTeachPoint, void(float,float,float,float,float,float, float)); 213 | }; 214 | 215 | class ControllerTest: public Test { 216 | public: 217 | ControllerTest(void) { 218 | m_controller.curve(BASE ).stop( 45); 219 | m_controller.curve(SHOULDER).stop(-10); 220 | m_controller.curve(ELBOW ).stop( 20); 221 | m_controller.curve(ROLL ).stop( 30); 222 | m_controller.curve(PITCH ).stop( 15); 223 | m_controller.curve(WRIST ).stop(-20); 224 | m_controller.curve(GRIPPER ).stop( 70); 225 | } 226 | protected: 227 | void send(const char *str) { 228 | while (*str) 229 | m_controller.parseChar(*str++); 230 | }; 231 | MockController m_controller; 232 | }; 233 | 234 | TEST_F(ControllerTest, CheckTime) { 235 | EXPECT_CALL(m_controller, reportTime()); 236 | send("t"); 237 | } 238 | 239 | TEST_F(ControllerTest, ReportBase) { 240 | EXPECT_CALL(m_controller, reportAngle(45)); 241 | send("b"); 242 | } 243 | 244 | TEST_F(ControllerTest, ReportBasePWM) { 245 | EXPECT_CALL(m_controller, reportPWM(2040)); 246 | send("B"); 247 | } 248 | 249 | TEST_F(ControllerTest, ClipAcceptsValues) { 250 | EXPECT_EQ(1500, m_controller.clipPWM(BASE, 1500)); 251 | } 252 | 253 | TEST_F(ControllerTest, ClipLowerBound) { 254 | EXPECT_EQ(544, m_controller.clipPWM(BASE, 500)); 255 | } 256 | 257 | TEST_F(ControllerTest, ClipUpperBound) { 258 | EXPECT_EQ(2400, m_controller.clipPWM(BASE, 2500)); 259 | } 260 | 261 | TEST_F(ControllerTest, ConvertZeroAngleToPWM) { 262 | EXPECT_EQ(1500, m_controller.angleToPWM(SHOULDER, 0)); 263 | } 264 | 265 | TEST_F(ControllerTest, ConvertAngleToPWM) { 266 | EXPECT_EQ(1740, m_controller.angleToPWM(SHOULDER, 20)); 267 | } 268 | 269 | TEST_F(ControllerTest, ConvertCenterPWMToAngle) { 270 | EXPECT_EQ(0, m_controller.pwmToAngle(BASE, 1500)); 271 | } 272 | 273 | TEST_F(ControllerTest, ConvertPWMToAngle) { 274 | EXPECT_EQ(20, m_controller.pwmToAngle(BASE, 1740)); 275 | } 276 | 277 | TEST_F(ControllerTest, TargetBaseAngle) { 278 | send("12b"); 279 | EXPECT_EQ(12, m_controller.curve(BASE).target()); 280 | } 281 | 282 | TEST_F(ControllerTest, TargetBasePWM) { 283 | send("1320B"); 284 | EXPECT_EQ(-15.0, m_controller.curve(BASE).target()); 285 | } 286 | 287 | TEST_F(ControllerTest, RetargetBaseFloat) { 288 | send("12.5b"); 289 | EXPECT_EQ(12.5, m_controller.curve(BASE).target()); 290 | } 291 | 292 | TEST_F(ControllerTest, TargetBasePWMFloat) { 293 | send("1501.5B"); 294 | EXPECT_EQ(0.125, m_controller.curve(BASE).target()); 295 | } 296 | 297 | TEST_F(ControllerTest, TargetBasePWMLowerLimit) { 298 | send("0B"); 299 | EXPECT_LT(-80, m_controller.curve(BASE).target()); 300 | } 301 | 302 | TEST_F(ControllerTest, TargetShoulderPWMLimitAgainstElbow) { 303 | send("2400S"); 304 | EXPECT_GE(40, m_controller.curve(SHOULDER).target()); 305 | } 306 | 307 | TEST_F(ControllerTest, RetargetNegativeNumber) { 308 | send("-1.5b"); 309 | EXPECT_EQ(-1.5, m_controller.curve(BASE).target()); 310 | } 311 | 312 | TEST_F(ControllerTest, DoubleMinus) { 313 | send("--1b"); 314 | EXPECT_EQ(1, m_controller.curve(BASE).target()); 315 | } 316 | 317 | TEST_F(ControllerTest, MinusClearsNumber) { 318 | send(".5-3b"); 319 | EXPECT_EQ(-3, m_controller.curve(BASE).target()); 320 | } 321 | 322 | TEST_F(ControllerTest, RetargetZero) { 323 | send("0b"); 324 | EXPECT_EQ(0, m_controller.curve(BASE).target()); 325 | } 326 | 327 | TEST_F(ControllerTest, TargetTwice) { 328 | send("3b3b"); 329 | EXPECT_EQ(3, m_controller.curve(BASE).target()); 330 | } 331 | 332 | TEST_F(ControllerTest, IgnoreInvalidFloat) { 333 | send("3.4.5b"); 334 | EXPECT_EQ(5, m_controller.curve(BASE).target()); 335 | } 336 | 337 | TEST_F(ControllerTest, AbortRetargetBase) { 338 | EXPECT_CALL(m_controller, reportAngle(45)); 339 | send("5xb"); 340 | EXPECT_EQ(45, m_controller.curve(BASE).target()); 341 | } 342 | 343 | TEST_F(ControllerTest, CorrectTarget) { 344 | send("5x3b"); 345 | EXPECT_EQ(3, m_controller.curve(BASE).target()); 346 | } 347 | 348 | TEST_F(ControllerTest, RetargetBaseOnce) { 349 | EXPECT_CALL(m_controller, reportAngle(45)); 350 | send("30bb"); 351 | EXPECT_EQ(30, m_controller.curve(BASE).target()); 352 | } 353 | 354 | TEST_F(ControllerTest, ReportElbow) { 355 | EXPECT_CALL(m_controller, reportAngle(20)); 356 | send("e"); 357 | } 358 | 359 | TEST_F(ControllerTest, ReportElbowPWM) { 360 | EXPECT_CALL(m_controller, reportPWM(1740)); 361 | send("E"); 362 | } 363 | 364 | TEST_F(ControllerTest, RetargetElbow) { 365 | send("10e"); 366 | EXPECT_EQ(10, m_controller.curve(ELBOW).target()); 367 | } 368 | 369 | TEST_F(ControllerTest, ReportShoulder) { 370 | EXPECT_CALL(m_controller, reportAngle(-10)); 371 | send("s"); 372 | } 373 | 374 | TEST_F(ControllerTest, ReportShoulderPWM) { 375 | EXPECT_CALL(m_controller, reportPWM(1380)); 376 | send("S"); 377 | } 378 | 379 | TEST_F(ControllerTest, ReportRoll) { 380 | EXPECT_CALL(m_controller, reportAngle(30)); 381 | send("r"); 382 | } 383 | 384 | TEST_F(ControllerTest, ReportRollPWM) { 385 | EXPECT_CALL(m_controller, reportPWM(1860)); 386 | send("R"); 387 | } 388 | 389 | TEST_F(ControllerTest, ReportPitch) { 390 | EXPECT_CALL(m_controller, reportAngle(15)); 391 | send("p"); 392 | } 393 | 394 | TEST_F(ControllerTest, ReportPitchPWM) { 395 | EXPECT_CALL(m_controller, reportPWM(1680)); 396 | send("P"); 397 | } 398 | 399 | TEST_F(ControllerTest, ReportWrist) { 400 | EXPECT_CALL(m_controller, reportAngle(-20)); 401 | send("w"); 402 | } 403 | 404 | TEST_F(ControllerTest, ReportWristPWM) { 405 | EXPECT_CALL(m_controller, reportPWM(1260)); 406 | send("W"); 407 | } 408 | 409 | TEST_F(ControllerTest, ReportGripper) { 410 | EXPECT_CALL(m_controller, reportAngle(70)); 411 | send("g"); 412 | } 413 | 414 | TEST_F(ControllerTest, ReportGripperPWM) { 415 | EXPECT_CALL(m_controller, reportPWM(2340)); 416 | send("G"); 417 | } 418 | 419 | TEST_F(ControllerTest, UseBase) { 420 | EXPECT_EQ(-90, m_controller.limitArmAngle(BASE, -90)); 421 | EXPECT_EQ( 90, m_controller.limitArmAngle(BASE, 90)); 422 | } 423 | 424 | TEST_F(ControllerTest, RestrictElbow) { 425 | m_controller.curve(SHOULDER).stop(0); 426 | EXPECT_EQ( 60, m_controller.limitArmAngle(ELBOW, 70)); 427 | EXPECT_EQ(-60, m_controller.limitArmAngle(ELBOW, -70)); 428 | } 429 | 430 | TEST_F(ControllerTest, RestrictElbowRelativeToShoulder) { 431 | 432 | EXPECT_EQ( 70, m_controller.limitArmAngle(ELBOW, 90)); 433 | EXPECT_EQ(-50, m_controller.limitArmAngle(ELBOW,-90)); 434 | } 435 | 436 | TEST_F(ControllerTest, UseElbowRestriction) { 437 | send("90e"); 438 | EXPECT_EQ(70, m_controller.curve(ELBOW).target()); 439 | } 440 | 441 | TEST_F(ControllerTest, RestrictShoulder) { 442 | m_controller.curve(ELBOW).stop(0); 443 | EXPECT_EQ( 60, m_controller.limitArmAngle(SHOULDER, 70)); 444 | EXPECT_EQ(-60, m_controller.limitArmAngle(SHOULDER,-70)); 445 | } 446 | 447 | TEST_F(ControllerTest, RestrictShoulderRelativeToElbow) { 448 | m_controller.curve(ELBOW).stop(-20); 449 | EXPECT_EQ( 80, m_controller.limitArmAngle(SHOULDER, 90)); 450 | EXPECT_EQ(-40, m_controller.limitArmAngle(SHOULDER,-90)); 451 | } 452 | 453 | TEST_F(ControllerTest, UpdateInformsServos) { 454 | m_controller.targetAngle(BASE, 0); 455 | EXPECT_CALL(m_controller, writePWM(BASE , 2040)); 456 | EXPECT_CALL(m_controller, writePWM(SHOULDER, 1380)); 457 | EXPECT_CALL(m_controller, writePWM(ELBOW , 1740)); 458 | EXPECT_CALL(m_controller, writePWM(ROLL , 1860)); 459 | EXPECT_CALL(m_controller, writePWM(PITCH , 1680)); 460 | EXPECT_CALL(m_controller, writePWM(WRIST , 1260)); 461 | EXPECT_CALL(m_controller, writePWM(GRIPPER , 2340)); 462 | m_controller.update(0); 463 | } 464 | 465 | TEST_F(ControllerTest, UpdateAppliesTargets) { 466 | m_controller.targetAngle(BASE, 0); 467 | EXPECT_CALL(m_controller, writePWM(BASE , 1500)); 468 | EXPECT_CALL(m_controller, writePWM(SHOULDER, 1380)); 469 | EXPECT_CALL(m_controller, writePWM(ELBOW , 1740)); 470 | EXPECT_CALL(m_controller, writePWM(ROLL , 1860)); 471 | EXPECT_CALL(m_controller, writePWM(PITCH , 1680)); 472 | EXPECT_CALL(m_controller, writePWM(WRIST , 1260)); 473 | EXPECT_CALL(m_controller, writePWM(GRIPPER , 2340)); 474 | m_controller.update(10); 475 | } 476 | 477 | TEST_F(ControllerTest, StopDrives) { 478 | for (int i=0; i