def run():
roboveroConfig()
MCPWM_Init(LPC_MCPWM)
channelsetup = MCPWM_CHANNEL_CFG_Type()
channelsetup.channelType = MCPWM_CHANNEL_EDGE_MODE
channelsetup.channelPolarity = MCPWM_CHANNEL_PASSIVE_HI
channelsetup.channelDeadtimeEnable = DISABLE
channelsetup.channelDeadtimeValue = 0
channelsetup.channelUpdateEnable = ENABLE
channelsetup.channelTimercounterValue = 0
channelsetup.channelPeriodValue = periodValue
channelsetup.channelPulsewidthValue = 0
MCPWM_ConfigChannel(LPC_MCPWM, 0, channelsetup.ptr)
# Disable DC Mode
MCPWM_DCMode(LPC_MCPWM, DISABLE, ENABLE, (0))
# Disable AC Mode
MCPWM_ACMode(LPC_MCPWM, DISABLE)
MCPWM_Start(LPC_MCPWM, ENABLE, DISABLE, DISABLE)
try:
while True:
channelsetup.channelPulsewidthValue = getMotorSpeed()
MCPWM_WriteToShadow(LPC_MCPWM, 0, channelsetup.ptr)
except:
MCPWM_Stop(LPC_MCPWM, ENABLE, DISABLE, DISABLE)
print "you broke it"
def run():
roboveroConfig()
dataA = [0, 10, 20, 30]
dataB = [40, 50, 60, 70]
TXMsg = CAN_MSG_Type()
TXMsg.format = CAN_ID_FORMAT_Type.EXT_ID_FORMAT
TXMsg.id = 0x00000000
TXMsg.len = 8
TXMsg.type = CAN_FRAME_Type.DATA_FRAME
TXMsg.dataA = dataA
TXMsg.dataB = dataB
CAN_Init(LPC_CAN1, 100000)
CAN_SetAFMode(LPC_CANAF, CAN_AFMODE_Type.CAN_AccBP)
count = 0
while True:
sys.stdout.write("\r" + chr(27) + "[2K")
sys.stdout.write("Messages sent: %d" % count)
sys.stdout.flush()
CAN_SendMsg(LPC_CAN1, TXMsg.ptr)
for i in range(0,4):
dataA[i] += 1
dataB[i] += 1
TXMsg.dataA = dataA
TXMsg.dataB = dataB
count += 1
time.sleep(1)
def run():
roboveroConfig()
RoboCaller.call('PWM_SetBasePeriod', 'void', 2000)
RoboCaller.call('PWM_SetSpeed', 'void', 1, 10)
while True:
match_value = getServoAngle()
if match_value:
RoboCaller.call('PWM_SetSpeed', 'void', 1, match_value)
def __init__(self): # Initialize pin select registers roboveroConfig() # enable IMU_EN pinMode(P1_0, OUTPUT) digitalWrite(P1_0, 0) self.accelerometer = Accelerometer() self.compass = Compass() self.gyrometer = Gyrometer()
def run():
roboveroConfig()
while True:
_msg = raw_input("enter a message:")
msg = Array(len(_msg), 1, _msg)
UART_Send(LPC_UART1, msg.ptr, msg.length, TRANSFER_BLOCK_Type.BLOCKING)
print _msg
time.sleep(1)
def run(): # Entry Point roboveroConfig() initPWM() while True: match_value = getServoAngle() if match_value: PWM_MatchUpdate(LPC_PWM1, 1, match_value, PWM_MATCH_UPDATE_OPT.PWM_MATCH_UPDATE_NOW)
def run():
heartbeatOff();
roboveroConfig()
MCPWM_Init(LPC_MCPWM)
channelsetup=[]
for i in range(3):
channelsetup.append(MCPWM_CHANNEL_CFG_Type())
channelsetup[0].channelType = MCPWM_CHANNEL_EDGE_MODE
channelsetup[0].channelPolarity = MCPWM_CHANNEL_PASSIVE_LO
channelsetup[0].channelDeadtimeEnable = FunctionalState.DISABLE
channelsetup[0].channelDeadtimeValue = 0
channelsetup[0].channelUpdateEnable = FunctionalState.ENABLE
channelsetup[0].channelTimercounterValue = 0
channelsetup[0].channelPeriodValue = 900
channelsetup[0].channelPulsewidthValue = 450
channelsetup[1].channelType = MCPWM_CHANNEL_EDGE_MODE
channelsetup[1].channelPolarity = MCPWM_CHANNEL_PASSIVE_LO
channelsetup[1].channelDeadtimeEnable = FunctionalState.DISABLE
channelsetup[1].channelDeadtimeValue = 0
channelsetup[1].channelUpdateEnable = FunctionalState.ENABLE
channelsetup[1].channelTimercounterValue = 0
channelsetup[1].channelPeriodValue = 900
channelsetup[1].channelPulsewidthValue = 200
channelsetup[2].channelType = MCPWM_CHANNEL_EDGE_MODE
channelsetup[2].channelPolarity = MCPWM_CHANNEL_PASSIVE_LO
channelsetup[2].channelDeadtimeEnable = FunctionalState.DISABLE
channelsetup[2].channelDeadtimeValue = 0
channelsetup[2].channelUpdateEnable = FunctionalState.ENABLE
channelsetup[2].channelTimercounterValue = 0
channelsetup[2].channelPeriodValue = 900
channelsetup[2].channelPulsewidthValue = 200
MCPWM_ConfigChannel(LPC_MCPWM, 0, channelsetup[0].ptr)
MCPWM_ConfigChannel(LPC_MCPWM, 1, channelsetup[1].ptr)
MCPWM_ConfigChannel(LPC_MCPWM, 2, channelsetup[2].ptr)
#DC Mode
MCPWM_DCMode(LPC_MCPWM, FunctionalState.ENABLE, FunctionalState.DISABLE, (0))
MCPWM_Start(LPC_MCPWM, FunctionalState.ENABLE, FunctionalState.ENABLE, FunctionalState.ENABLE)
try:
while True:
#MCPWM_WriteToShadow(LPC_MCPWM, 0, channelsetup[0].ptr) #Change the speed
for i in range(6):
BLDC_commutate(i)
except:
MCPWM_Stop(LPC_MCPWM, FunctionalState.ENABLE, FunctionalState.ENABLE, FunctionalState.ENABLE)
heartbeatOn()
print "you broke it"
def __init__(self, config):
roboveroConfig()
self.motors = MotorController(config)
self.cam = VidServer(config)
self.ultrasonic_sensor = UltraSonic(config)
self.ir_sensor = IR(config)
self.target_altitude = config.getfloat("control", "target_altitude")
self.left_clearance = config.getfloat("control", "left_clearance")
self.right_clearance = config.getfloat("control", "right_clearance")
self.pulse_hi = config.getint("control", "pulse_hi")
self.pulse_low = config.getint("control", "pulse_low")
self.pulse_arm = config.getint("control", "pulse_arm")
self.left_channel = config.getint("control", "left_channel")
self.right_channel = config.getint("control", "right_channel")
self.bottom_channel = config.getint("control", "bottom_channel")
def run(): roboveroConfig() RXMsg = CAN_MSG_Type() RXMsg.format = 0x00 RXMsg.id = 0x00 RXMsg.len = 0x00 RXMsg.type = 0x00 CAN_Init(LPC_CAN1, 100000) CAN_SetAFMode(LPC_CANAF, CAN_AFMODE_Type.CAN_AccBP) while True: if CAN_ReceiveMsg(LPC_CAN1, RXMsg.ptr): print RXMsg.dataA, RXMsg.dataB time.sleep(0.1)
def run():
roboveroConfig()
try:
while True:
data = [
analogRead(AD0_0),
analogRead(AD0_1),
analogRead(AD0_2),
analogRead(AD0_3),
analogRead(AD0_5),
analogRead(AD0_6),
analogRead(AD0_7)
]
print data
except KeyboardInterrupt:
exit("\nkeyboard interrupt: how rude!")
def run():
roboveroConfig()
UARTFIFOConfigStruct = UART_FIFO_CFG_Type()
UART_FIFOConfigStructInit(UARTFIFOConfigStruct.ptr)
UART_FIFOConfig(LPC_UART1, UARTFIFOConfigStruct.ptr);
msg = Array(1, 1, "")
try:
while True:
length = UART_Receive(LPC_UART1, msg.ptr, 1, TRANSFER_BLOCK_Type.NONE_BLOCKING)
if length!=0:
print chr(msg[0])
time.sleep(1)
except:
exit("goodbye")
def run():
roboveroConfig()
# draw a listbox
root = Tk()
root.title("Tone Generator")
# populate the list with available frequencies
tone_listbox = Listbox(root)
tone_listbox.insert(END, "OFF")
for freq in range(100, 1000, 100):
tone_listbox.insert(END, "%d Hz" % freq)
tone_listbox.pack()
# set the callback function for left button clicks
tone_listbox.bind('<ButtonRelease-1>', playSelection)
root.mainloop()
def theDriver():
global throttle
global steering
rospy.init_node(NODE_NAME)
rate = rospy.Rate(LOOP_FREQ)
rospy.Subscriber(CAR_PUBLISH_TOPIC, Twist, carCommandCallback)
roboveroConfig()
initPWM1()
initPWM2()
time.sleep(3)
while not rospy.is_shutdown():
#main loop
PWM_MatchUpdate(LPC_PWM1, 1, getServoValue(steering), PWM_MATCH_UPDATE_OPT.PWM_MATCH_UPDATE_NOW)
PWM_MatchUpdate(LPC_PWM1, 2, getServoValue(throttle), PWM_MATCH_UPDATE_OPT.PWM_MATCH_UPDATE_NOW)
rospy.loginfo("throttle=%f, steering=%f" % (throttle, steering) )
rate.sleep()
def run():
roboveroConfig()
pinMode(LED, OUTPUT)
heartbeatOff()
# Initialize script
myscript=Script([["GPIO_ClearValue", 3, 2000000],
["delay",10000000],
["GPIO_SetValue", 3, 2000000]])
print "script added"
try:
while True:
raw_input("Press Enter to run script")
print myscript.run()
print "script done"
# append to script
myscript.append("delay",10000000)
myscript.append("GPIO_ClearValue", 3, 2000000)
myscript.append("delay",10000000)
myscript.append("GPIO_SetValue",3, 2000000)
except:
heartbeatOn()
print "goodbye"
from robovero.internals import robocaller, store_data #store_data is used to write the data to a file from robovero.extras import roboveroConfig print 'Robot configuration' roboveroConfig() from uart2 import UARTInit, UARTConvert, FloatToBytes from adc2 import ADCConvert, BatteryVoltage, DistanceInit from IMU3 import IMUInit, IMUConvert from math import pi, degrees, radians, cos, sin, sqrt, hypot, atan2 from algorithm import Normalize, Algorithm, DistanceControl, HeadingControl import time #------ Global Variables ------- #Power voltage = 0 current = 0 filtered_voltage = 12 #Battery voltage of 12V at the beginning energy_consumme = 0.0 #Odometry speed = 0.0 rot_speed = 0.0 heading = 0.0 direction = heading pose_x = 0 pose_y = 0 odom = [0, 0, 0, 0] speed_odom = 0 rot_speed_odom = 0 pose_x_odom = 0 pose_y_odom = 0 heading_odom = 0 direction_odom = heading_odom
def writeReg(self, register, value):
self.tx_data[0] = register
self.tx_data[1] = value
self.config.tx_length = 2
self.config.rx_data = 0
self.config.rx_length = 0
ret = I2C_MasterTransferData(LPC_I2C0, self.config.ptr,
I2C_TRANSFER_OPT_Type.I2C_TRANSFER_POLLING)
if ret == Status.ERROR:
exit("I2C Write Error")
if self.readReg(register) != value:
exit("I2C Verification Error")
return None
# Initialize pin select registers
roboveroConfig()
# configure accelerometer
accelerometer = I2CDevice(0x18)
accelerometer.writeReg(accel_ctrl_reg1, 0x27)
accelerometer.writeReg(accel_ctrl_reg4, 0x00) #
# configure compass
compass = I2CDevice(0x1E)
compass.writeReg(compass_cra_reg, 0x18) # 75 Hz
compass.writeReg(compass_crb_reg, 0x20) # +/- 1.3 gauss
compass.writeReg(compass_mr_reg, 0) # continuous measurement mode
# configure the gyro
# from the L3G4200D Application Note:
# 1. Write CTRL_REG2
| 3-------1 | 2 Also 0->triangle, 1->circle, 2->X, 3-> square """ #enter in speed as percentage and the correct pulse width for the motor is returned def byte_to_pulse(byte): byte=min(max(byte,ZERO),MAX) #makes sure speed between ZERO and MAX pulse=8*byte-40 #scales bit value to pulse: ZERO (130) leads to 1000 ms pulse, MAX (255) leads to a 2000 ms pulse return pulse roboveroConfig() #configures robovero (not sure exactly what this does) #sets up pwm with the period, the pulse width and then gives the motor controller a high, a low and a mid signal and then low one again, so it begins stopped initPWM() #This loop gets the data from Ps2 then pulses motors accordingly Ps2=Ps2Control() while (1): Ps2.refresh() """Get data from ps2 controller""" stick=Ps2.read_sticks() shapes=Ps2.read_shape()
def __init__(self):
# open serial port
#self.device = rospy.get_param('~device', '/dev/ttyUSB0')
#self.baud = rospy.get_param('~baud', 38400)
#self.ser = serial.Serial(self.device, self.baud, timeout=1)
# reset variables
#self.orientation = 0
#self.bias = 0
# Initialize pin select registers
roboveroConfig()
print "roboveroConfig"
# configure accelerometer
self.accelerometer = I2CDevice(0x18)
self.accelerometer.writeReg(accel_ctrl_reg1, 0x27)
self.accelerometer.writeReg(accel_ctrl_reg4, 0x00) #
# configure compass
self.compass = I2CDevice(0x1E)
self.compass.writeReg(compass_cra_reg, 0x18) # 75 Hz
self.compass.writeReg(compass_crb_reg, 0x20) # +/- 1.3 gauss
self.compass.writeReg(compass_mr_reg, 0) # continuous measurement mode
# configure the gyro
# from the L3G4200D Application Note:
# 1. Write CTRL_REG2
# 2. Write CTRL_REG3
# 3. Write CTRL_REG4
# 4. Write CTRL_REG6
# 5. Write Reference
# 6. Write INT1_THS
# 7. Write INT1_DUR
# 8. Write INT1_CFG
# 9. Write CTRL_REG
# 10. Write CTRL_REG
self.gyro = I2CDevice(0x68)
self.gyro.writeReg(gyro_ctrl_reg3, 0x08) # enable DRDY
self.gyro.writeReg(gyro_ctrl_reg4, 0x80) # enable block data read mode
self.gyro.writeReg(gyro_ctrl_reg1, 0x0F) # normal mode, enable all axes, 250dps
self.calibrating = False
self.frame_id = 'base_footprint'
self.prev_time = rospy.Time.now()
# publisher with imu data
self.pub = rospy.Publisher("/robovero/imu/data", Imu)
# rotation scale
self.scale = rospy.get_param('~rot_scale', 1.0)
# service for calibrating gyro bias
rospy.Service("/robovero/imu/calibrate", Empty, self.calibrateCallback)
# publisher with calibration state
self.is_calibratedPublisher = rospy.Publisher('/robovero/imu/is_calibrated', Bool, latch=True)
# We'll always just reuse this msg object:
self.is_CalibratedResponseMsg = Bool();
# Initialize the latched is_calibrated state.
# At the beginning calibration is assumed to be done
self.is_CalibratedResponseMsg.data = True;
self.is_calibratedPublisher.publish(self.is_CalibratedResponseMsg)
def run():
# Initialize pin select registers
roboveroConfig()
# Initialize IMU_EN
IMUInit()
# configure accelerometer
accelerometer = I2CDevice(0x18)
accelerometer.writeReg(accel_ctrl_reg1, 0x27)
accelerometer.writeReg(accel_ctrl_reg4, 0x00) #
# configure compass
compass = I2CDevice(0x1E)
compass.writeReg(compass_cra_reg, 0x18) # 75 Hz
compass.writeReg(compass_crb_reg, 0x20) # +/- 1.3 gauss
compass.writeReg(compass_mr_reg, 0) # continuous measurement mode
# configure the gyro
# from the L3G4200D Application Note:
# 1. Write CTRL_REG2
# 2. Write CTRL_REG3
# 3. Write CTRL_REG4
# 4. Write CTRL_REG6
# 5. Write Reference
# 6. Write INT1_THS
# 7. Write INT1_DUR
# 8. Write INT1_CFG
# 9. Write CTRL_REG
# 10. Write CTRL_REG
gyro = I2CDevice(0x68)
gyro.writeReg(gyro_ctrl_reg3, 0x08) # enable DRDY
gyro.writeReg(gyro_ctrl_reg4, 0x80) # enable block data read mode
gyro.writeReg(gyro_ctrl_reg1, 0x0F) # normal mode, enable all axes, 250dps
while True:
print "time: ",
print time.time()
acceldata = accelerometer.read6Reg(accel_x_low)
compassdata = compass.read6Reg(compass_x_high)
gyrodata = gyro.read6Reg(gyro_x_low)
print "a [x, y, z]: ",
print [
twosComplement(acceldata[0], acceldata[1]), #/16384.0,
twosComplement(acceldata[2], acceldata[3]), #/16384.0,
twosComplement(acceldata[4], acceldata[5]) #/16384.0
]
print "c [x, y, z]: ",
print [
twosComplement(compassdata[1], compassdata[0]), #/1055.0,
twosComplement(compassdata[3], compassdata[2]), #/1055.0,
twosComplement(compassdata[5], compassdata[4]) #/950.0
]
print "g [x, y, z]: ",
print [
twosComplement(gyrodata[0], gyrodata[1]),
twosComplement(gyrodata[2], gyrodata[3]),
twosComplement(gyrodata[4], gyrodata[5])
]
