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.
'''

import time
import jetduino
from jetduino_pins import *

servo = 1

print ("setting return status level to 1")
jetduino.dynamixelSetRegister(servo, jetduino.AX_RETURN_LEVEL, 1, 1)

ret_level = jetduino.dynamixelGetRegister(servo, jetduino.AX_RETURN_LEVEL, 1)
print ("return status level: %d" % (ret_level))

#first get the angle limits
cw_limit_orig = jetduino.dynamixelGetRegister(servo, jetduino.AX_CW_ANGLE_LIMIT_L, 2)
ccw_limit_orig = jetduino.dynamixelGetRegister(servo, jetduino.AX_CCW_ANGLE_LIMIT_L, 2)
print ("Before: CW Limit: %d, CCW Limit: %d" % (cw_limit_orig, ccw_limit_orig))

#now set them to something else
print ("Setting cw=130, ccw=180")
jetduino.dynamixelSetRegister(servo, jetduino.AX_CW_ANGLE_LIMIT_L, 2, 130)
jetduino.dynamixelSetRegister(servo, jetduino.AX_CCW_ANGLE_LIMIT_L, 2, 800)

#get the angle limits again to check
cw_limit = jetduino.dynamixelGetRegister(servo, jetduino.AX_CW_ANGLE_LIMIT_L, 2)
ccw_limit = jetduino.dynamixelGetRegister(servo, jetduino.AX_CCW_ANGLE_LIMIT_L, 2)
print("Move to 1023 slowly")
jetduino.dynamixelMove(servo, 1023, 200)
time.sleep(1)

print("stop the servo at its current position.")
jetduino.dynamixelStop(servo)
time.sleep(1)

while True:
    try:
        print("Moving to 1023 at 100")
        jetduino.dynamixelMove(servo, 1023, 100)

        for num in range(1, 30):
            pos = jetduino.dynamixelGetRegister(servo, jetduino.AX_PRESENT_POSITION_L, 2)
            print("Pos: %d" % pos)

        print("Moving to 10 at 1000")
        jetduino.dynamixelMove(servo, 10, 1000)

        for num in range(1, 30):
            pos = jetduino.dynamixelGetRegister(servo, jetduino.AX_PRESENT_POSITION_L, 2)
            print("Pos: %d" % pos)

    except KeyboardInterrupt:
        print("Exiting loop")
        break
    except IOError:
        print("Error")