;;; **GRL OUTPUT FILE | Ver. 1.21**
;;; FILENAME: grloutput.scm
;;; CREATED: Tue Dec 12 15:58:44 2000
;;; GENERATED BY: Alan Kendall & Prashant Velagaleti


;;; Data & Calculations: (verbose)
;; numnodes = 7

;; distance 0 = 0
;; angle 0    = 0

;; distance 1 = 1342.38593556399
;; angle 1    = 298.474203610074

;; distance 2 = 1640
;; angle 2    = 298.779806304397

;; distance 3 = 1620.12345208629
;; angle 3    = 60.710488408115

;; distance 4 = 1626.03812993423
;; angle 4    = 58.0693178962822

;; distance 5 = 1640
;; angle 5    = 64.9325365619395

;; distance 6 = 1368.94119669181
;; angle 6    = 0

(define-action (phase0-translation)
		(terminate-when (>= base-x 1342.38593556399))
		(rt-vector 0 4000))

(define-action (phase1-rotation)
;; Negative rotation...
		(terminate-when (and (<= base-orientation 5.20935758526374)
				         (> base-orientation 3.14159265358979)))
		(rt-vector -2000 0))

(define-action (phase1-translation)
		(terminate-when (>= base-x 1640))
		(rt-vector 0 5000))

(define-action (phase2-rotation)
;; Negative rotation...
		(terminate-when (and (<= base-orientation 5.21469135848264)
				         (> base-orientation 3.14159265358979)))
		(rt-vector -2000 0))

(define-action (phase2-translation)
		(terminate-when (>= base-x 1620.12345208629))
		(rt-vector 0 3000))

(define-action (phase3-rotation)
;; Positive rotation...
		(terminate-when (and (>= base-orientation 1.05959791321546)
				         (< base-orientation 3.14159265358979)))
		(rt-vector 2000 0))

(define-action (phase3-translation)
		(terminate-when (>= base-x 1626.03812993423))
		(rt-vector 0 3000))

(define-action (phase4-rotation)
;; Positive rotation...
		(terminate-when (and (>= base-orientation 1.01350079167739)
				         (< base-orientation 3.14159265358979)))
		(rt-vector 2000 0))

(define-action (phase4-translation)
		(terminate-when (>= base-x 1640))
		(rt-vector 0 5000))

(define-action (phase5-rotation)
;; Positive rotation...
		(terminate-when (and (>= base-orientation 1.13328655467744)
				         (< base-orientation 3.14159265358979)))
		(rt-vector 2000 0))

(define-action (phase5-translation)
		(terminate-when (>= base-x 1368.94119669181))
		(rt-vector 0 4000))

(define-plan (gui-plan)
		(initially-running? #t)
		(scheme (reset-base-odometry!))
		(phase0-translation)
		(scheme (reset-base-odometry!))
		(phase1-rotation)
		(phase1-translation)
		(scheme (reset-base-odometry!))
		(phase2-rotation)
		(phase2-translation)
		(scheme (reset-base-odometry!))
		(phase3-rotation)
		(phase3-translation)
		(scheme (reset-base-odometry!))
		(phase4-rotation)
		(phase4-translation)
		(scheme (reset-base-odometry!))
		(phase5-rotation)
		(phase5-translation))

(try-signals (reset-odometry-on-startup)
		gui-plan
		(drive-base phase0-translation phase1-rotation phase1-translation phase2-rotation phase2-translation phase3-rotation phase3-translation phase4-rotation phase4-translation phase5-rotation phase5-translation)
		(trace-value base-x)
		(trace-value base-orientation))