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]) ]