gyroboy.go

/*
 * gyroboy.go - translation of the LEGO Education GyroBoy program to go for ev3dev
 *
 * MIT License
 *
 * Copyright (c) 2016 David Lechner <david@lechnology.com>
 *
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
 * SOFTWARE.
 */

package main

import (
	"fmt"
	"io"
	"io/ioutil"
	"math"
	"os"
	"path"
	"path/filepath"
	"strconv"
	"strings"
	"time"
)

const (
	offsetDampingFactor = 5 * time.Millisecond
)

type Sensor struct {
	mode_path   string
	value0_file *os.File
}

func (s *Sensor) SetMode(mode string) {
	err := ioutil.WriteFile(s.mode_path, []byte(mode), 0)
	if err != nil {
		panic(err.Error())
	}
}

func (s *Sensor) GetValue() int {
	v := make([]byte, 4096)
	size, err := s.value0_file.ReadAt(v, 0)
	if err != io.EOF && err != nil {
		panic(err.Error())
	}
	r, err := strconv.Atoi(strings.TrimSpace(string(v[:size])))
	if err != nil {
		panic(err.Error())
	}
	return r
}

var color_sensor *Sensor
var gyro_sensor *Sensor
var touch_sensor *Sensor
var ultrasonic_sensor *Sensor

type SensorNotFoundError struct {
	Name    string
	Address string
}

func (e *SensorNotFoundError) Error() string {
	return fmt.Sprintf("Could not find '%s' sensor at '%s'", e.Name, e.Address)
}

func initSensor(name, address string) (s *Sensor, err error) {
	dirs, err := filepath.Glob("/sys/class/lego-sensor/sensor*")
	if err != nil {
		return nil, err
	}
	for _, d := range dirs {
		n, err := ioutil.ReadFile(path.Join(d, "driver_name"))
		if err != nil {
			return nil, err
		}
		if strings.TrimSpace(string(n)) != name {
			continue
		}
		a, err := ioutil.ReadFile(path.Join(d, "address"))
		if err != nil {
			return nil, err
		}
		if strings.TrimSpace(string(a)) != address {
			continue
		}
		value0_file, err := os.Open(path.Join(d, "value0"))
		if err != nil {
			return nil, err
		}
		return &Sensor{path.Join(d, "mode"), value0_file}, nil
	}
	return nil, &SensorNotFoundError{name, address}
}

func initSensors() {
	var err error
	color_sensor, err = initSensor("lego-ev3-color", "in1")
	if err != nil {
		panic(err.Error())
	}
	color_sensor.SetMode("COL-COLOR")
	gyro_sensor, err = initSensor("lego-ev3-gyro", "in2")
	if err != nil {
		panic(err.Error())
	}
	gyro_sensor.SetMode("GYRO-RATE")
	touch_sensor, err = initSensor("lego-ev3-touch", "in3")
	if err != nil {
		panic(err.Error())
	}
	touch_sensor.SetMode("TOUCH")
	ultrasonic_sensor, err = initSensor("lego-ev3-us", "in4")
	if err != nil {
		panic(err.Error())
	}
	ultrasonic_sensor.SetMode("US-DIST-IN")
}

type Motor struct {
	command_path       string
	duty_cycle_sp_file *os.File
	position_file      *os.File
	stop_action_path   string
}

func (m *Motor) GetPosition() int {
	v := make([]byte, 4096)
	size, err := m.position_file.ReadAt(v, 0)
	if err != io.EOF && err != nil {
		panic(err.Error())
	}
	r, err := strconv.Atoi(strings.TrimSpace(string(v[:size])))
	if err != nil {
		panic(err.Error())
	}
	return r
}

func (m *Motor) SetDutyCycleSetpoint(pct int) {
	_, err := m.duty_cycle_sp_file.Write([]byte(strconv.Itoa(pct)))
	if err != nil {
		panic(err.Error())
	}
}

func (m *Motor) SendCommand(cmd string) {
	err := ioutil.WriteFile(m.command_path, []byte(cmd), 0)
	if err != nil {
		panic(err.Error())
	}
}

func (m *Motor) SetStopAction(cmd string) {
	err := ioutil.WriteFile(m.stop_action_path, []byte(cmd), 0)
	if err != nil {
		panic(err.Error())
	}
}

type MotorNotFoundError struct {
	Name    string
	Address string
}

func (e *MotorNotFoundError) Error() string {
	return fmt.Sprintf("Could not find '%s' motor at '%s'", e.Name, e.Address)
}

var left_motor *Motor
var arms_motor *Motor
var right_motor *Motor

func initMotor(name, address string) (m *Motor, err error) {
	dirs, err := filepath.Glob("/sys/class/tacho-motor/motor*")
	if err != nil {
		return nil, err
	}
	for _, d := range dirs {
		n, err := ioutil.ReadFile(path.Join(d, "driver_name"))
		if err != nil {
			return nil, err
		}
		if strings.TrimSpace(string(n)) != name {
			continue
		}
		a, err := ioutil.ReadFile(path.Join(d, "address"))
		if err != nil {
			return nil, err
		}
		if strings.TrimSpace(string(a)) != address {
			continue
		}
		duty_cycle_sp_file, err := os.OpenFile(path.Join(d, "duty_cycle_sp"),
			os.O_WRONLY, 0)
		if err != nil {
			return nil, err
		}
		position_file, err := os.Open(path.Join(d, "position"))
		if err != nil {
			return nil, err
		}
		return &Motor{
			path.Join(d, "command"),
			duty_cycle_sp_file,
			position_file,
			path.Join(d, "stop_action"),
		}, nil
	}
	return nil, &MotorNotFoundError{name, address}
}

func initMotors() {
	var err error
	left_motor, err = initMotor("lego-ev3-l-motor", "outA")
	if err != nil {
		panic(err.Error())
	}
	arms_motor, err = initMotor("lego-ev3-m-motor", "outC")
	if err != nil {
		panic(err.Error())
	}
	right_motor, err = initMotor("lego-ev3-l-motor", "outD")
	if err != nil {
		panic(err.Error())
	}
}

// These are the global variables
var gyro_angle float64        // gAng
var state int                 // st
var fell_over bool            // ok
var motor_sum float64         // mSum
var motor_difference float64  // mDiff
var motor_position float64    // mPos
var motor_delta float64       // mD
var motor_delta_pos_1 float64 // mDP1
var motor_delta_pos_2 float64 // mDP2
var motor_delta_pos_3 float64 // mDP3
var motor_speed float64       // mSpd
var drive_control float64     // Cdrv
var loop_count int            // Clo
var power float64             // pwr
var steering_control float64  // Cstr
var gyro_offset float64       // gOS
var integral_time float64     // tInt
var gyro_speed float64        // gSpd
var timer1 time.Time
var timer2 time.Time

// This is the RST My Block
func reset() {
	fmt.Println("reset")
	left_motor.SendCommand("reset")
	right_motor.SendCommand("reset")
	// calling run_direct here so we don't waste time repeating it in the loop
	left_motor.SendCommand("run-direct")
	right_motor.SendCommand("run-direct")
	// TODO: reset gyro sensor - probably doesn't actually make a difference though
	timer2 = time.Now()
	motor_sum = 0
	motor_position = 0
	motor_delta = 0
	motor_delta_pos_1 = 0
	motor_delta_pos_2 = 0
	motor_delta_pos_3 = 0
	drive_control = 0
	loop_count = 0
	gyro_angle = 0
	fell_over = false
	power = 0
	state = 0
	steering_control = 0
	// official program sets cDrv (drive_control) twice - it is omitted here
}

// This is the gOS My Block
func calcGyroOffset() {
	fmt.Println("calcGyroOffset")
	var gyro_sum int // gSum
	var count int    // output of gChk loop
	// OSL loop
	for {
		gyro_min := 1000  // gMn
		gyro_max := -1000 // gMx
		gyro_sum = 0
		// gChk loop
		count = 200
		for count = 0; count < 200; count++ {
			gyro_rate := gyro_sensor.GetValue() // gyro
			gyro_sum += gyro_rate
			if gyro_rate > gyro_max {
				gyro_max = gyro_rate
			}
			if gyro_rate < gyro_min {
				gyro_min = gyro_rate
			}
			time.Sleep(4 * time.Millisecond)
		}
		if gyro_max-gyro_min < 2 {
			break
		}
	}
	gyro_offset = float64(gyro_sum) / float64(count)
	fmt.Println("gyro_offset:", gyro_offset)
}

// This is the GT My Block
func updateIntegralTime() {
	if loop_count == 0 {
		integral_time = 0.014
		timer1 = time.Now()
	} else {
		integral_time = time.Since(timer1).Seconds() / float64(loop_count)
	}
	loop_count += 1
}

// This is the GG My Block
func updateGyroAngle() {
	gyro_rate := float64(gyro_sensor.GetValue()) // gyro
	gyro_offset = offsetDampingFactor.Seconds() * gyro_rate
	gyro_offset += (time.Second - offsetDampingFactor).Seconds() * gyro_offset
	gyro_speed = gyro_rate - gyro_offset
	gyro_angle += gyro_speed * integral_time
}

// This is the GM My Block
func updateMotorPosition() {
	prev_motor_sum := motor_sum
	left_motor_pos := float64(left_motor.GetPosition())
	right_motor_pos := float64(right_motor.GetPosition())
	motor_sum = right_motor_pos + left_motor_pos
	motor_difference = right_motor_pos - left_motor_pos
	motor_delta = motor_sum - prev_motor_sum
	motor_position += motor_delta
	avg_delta := (motor_delta_pos_3 + motor_delta_pos_2 + motor_delta_pos_1 +
		motor_delta) / 4.0
	motor_speed = avg_delta / integral_time
	motor_delta_pos_3 = motor_delta_pos_2
	motor_delta_pos_2 = motor_delta_pos_1
	motor_delta_pos_1 = motor_delta
}

// This is the EQ My Block
func updatePidCalc() {
	motor_position -= integral_time * drive_control
	power = -0.01 * drive_control
	power += 0.08 * motor_speed
	power += 0.12 * motor_position
	power += 0.8 * gyro_speed
	power += 15 * gyro_angle
	if power > 100 {
		power = 100
	}
	if power < -100 {
		power = -100
	}
}

// This is the cntrl My Block
func doSpeedControl() (left, right int) {
	motor_position -= drive_control * integral_time
	steering := steering_control * 0.1
	return int(power - steering), int(power + steering)
}

// This is the CHK My Block
func checkIfFellOver() {
	if math.Abs(power) < 100 {
		timer2 = time.Now()
	}
	if time.Since(timer2).Seconds() > 1.0 {
		fell_over = true
	}
}

// This is the balancing sequence of blocks
func balance() {
	// Main (M) loop
	for {
		reset()
		// TODO: display sleeping eyes image
		calcGyroOffset()
		gyro_angle = -0.25
		// TODO: play speed up sound
		// TODO: play awake sound
		state = 1
		// Balance (BAL) loop
		fmt.Println("loop")
		for {
			updateIntegralTime()
			// start_time := time.Since(timer1)
			updateGyroAngle()
			updateMotorPosition()
			updatePidCalc()
			left_power, right_power := doSpeedControl()
			left_motor.SetDutyCycleSetpoint(left_power)
			right_motor.SetDutyCycleSetpoint(right_power)
			checkIfFellOver()
			// end_time := time.Since(timer1)
			// time.Sleep((5 * time.Millisecond) - (end_time - start_time))
			if fell_over {
				break
			}
		}
		fmt.Println("oops")
		fmt.Println("integral_time:", integral_time*1000)
		left_motor.SetStopAction("hold")
		right_motor.SetStopAction("hold")
		left_motor.SendCommand("stop")
		right_motor.SendCommand("stop")
		state = 0
		// TODO: make LEDs red, flashing
		// TODO: show knocked out eyes
		// TODO: play power down sound
		for touch_sensor.GetValue() == 0 {
			time.Sleep(10 * time.Millisecond)
		}
		for touch_sensor.GetValue() == 1 {
			time.Sleep(10 * time.Millisecond)
		}
		// TODO: reset LEDs
	}
}

func stop() {
	left_motor.SendCommand("reset")
	right_motor.SendCommand("reset")
	arms_motor.SendCommand("reset")
	// TODO: stop commands can be removed when motor driver is fixed
	left_motor.SendCommand("stop")
	right_motor.SendCommand("stop")
	arms_motor.SendCommand("stop")
}

func main() {
	initSensors()
	initMotors()
	defer stop()
	balance()
}