Esempio n. 1
0
    def new_joydata(self, joymsg):
        global Button
        global Axes
        #Should run at 100hz. Use launch files roslaunch crazyflie_ros joy.launch
        if self.prev_cmd == None:
            self.prev_cmd = joymsg
            print len(joymsg.axes)
            
            #USB mode
            if len(joymsg.axes) == 27:
                rospy.logwarn("Experimental: Using USB mode. Axes/Buttons may be different")
                Button = enum(L1=10,R1=11,Select=0,Start=3,L2=8,R2=9,Up=4,Right=5,Down=6,Left=7,Square=15,Cross=14,Triangle=12,Circle=13)
                Axes = enum(SLL=0,SLU=1,SRL=2,SRU=3,Up=4+4,Right=5+4,Down=6+4,Left=7+4,L2=4+8,R2=4+9,L1=4+10,R1=4+11,Triangle=4+12,Circle=4+13,Cross=4+14,Square=4+15,AccL=16,AccF=17,AccU=18,GyroY=19)
                self.USB = True
            return


        # bp = [i for i in range(len(joymsg.buttons)) if joymsg.buttons[i]]
        # ap = [(i,round(joymsg.axes[i],2)) for i in range(len(joymsg.axes)) if abs(joymsg.axes[i])>0.51 ]
        # if len(bp)>0:
        #     print "Buttons:", bp
        # if len(ap)>0:
        #     print "Axes   :", ap

        if self.USB:
            # USB buttons go from 1 to -1, Bluetooth go from 0 to -1, so normalise
            joymsg.axes =np.array(joymsg.axes, dtype=np.float32)
            joymsg.axes[8:19] -=1
            joymsg.axes[8:19] /=2
            # Strange bug: left button as axis doesnt work. So use button instead
            joymsg.axes[11] = -joymsg.buttons[Button.Left]/2.

        self.curr_cmd = joymsg
        hover = False

        x = 0
        y = 0
        z = 0
        r = 0
        r2 = 0
        # Get stick positions [-1 1]
        if self.curr_cmd.buttons[Button.L1]:  # Deadman pressed (=allow flight)
            x = self.joy_scale[0] * self.curr_cmd.axes[Axes.SLL] # Roll
            y = self.joy_scale[1] * self.curr_cmd.axes[Axes.SLU] # Pitch
            r = self.joy_scale[2] * self.curr_cmd.axes[Axes.SRL] # Yaw
            z = self.joy_scale[3] * self.curr_cmd.axes[Axes.SRU] # Thrust
            r2 = self.joy_scale[4] * (self.curr_cmd.axes[Axes.L2] - self.curr_cmd.axes[Axes.R2])
            hover = self.curr_cmd.axes[Axes.L1]<-0.75
        
        roll = x * self.max_angle
        pitch = y * self.max_angle        
        yaw = 0
        
        
        #TODO use dyn reconf to decide which to use
        if r2!=0:
            yaw = r2 * self.max_yawangle
        else:
            if self.yaw_joy:
                # Deadzone     
                if r < -0.2 or r > 0.2:
                    if r < 0:
                        yaw = (r + 0.2) * self.max_yawangle * 1.25
                    else:
                        yaw = (r - 0.2) * self.max_yawangle * 1.25
                
                
        if hover:
            thrust = int(round(deadband(z,0.2)*32767 + 32767)) #Convert to uint16
        else:    
            thrust = self.thurstToRaw(z)  
            
        trimmed_roll = roll + self.trim_roll
        trimmed_pitch = pitch + self.trim_pitch
        
            
        # Control trim manually
        new_settings = {}
        if joymsg.header.seq%10 == 0:

            if self.curr_cmd.buttons[Button.Left]:
                new_settings["trim_roll"] = max(self.trim_roll + self.curr_cmd.axes[Axes.Left], -20)
            if self.curr_cmd.buttons[Button.Right]:
                new_settings["trim_roll"] =  min(self.trim_roll - self.curr_cmd.axes[Axes.Right], 20)
            if self.curr_cmd.buttons[Button.Down]:
                new_settings["trim_pitch"] = max(self.trim_pitch + self.curr_cmd.axes[Axes.Down], -20)
            if self.curr_cmd.buttons[Button.Up]:
                new_settings["trim_pitch"] = min(self.trim_pitch - self.curr_cmd.axes[Axes.Up], 20)




        msg = CFJoyMSG()
        msg.header.stamp = rospy.Time.now()
        msg.roll = trimmed_roll
        msg.pitch = trimmed_pitch
        msg.yaw = yaw
        msg.thrust = thrust
        msg.hover = hover
        #msg.calib = self.pressed(Button.Triangle)



        # TODO: rotation is wrong! Cannot simply decompose x,y into r,p.
        # TODO: roll, pitch should be dependent from the distance and the angle.
        if self.PIDControl: # and hover:
            try:
                rtime = rospy.Time.now()

                # Update goal position
                if hover and joymsg.header.seq%10 == 0:
                    if self.PIDActive[0]:
                        if abs(x)>0.016:
                            new_settings["x"] = self.goal[0]-roll/70.
                        if abs(y)>0.016:
                            new_settings["y"] = self.goal[1]-pitch/70.

                    if self.PIDActive[1]:
                        if abs(z)>0.016:
                            new_settings["z"] = self.goal[2]+z/5

                    if self.PIDActive[2]:
                        if abs(yaw)>0:
                           new_settings["rz"] = self.goal[3]-yaw/self.max_yawangle*20

                    if  new_settings.has_key("x") or new_settings.has_key("y") or new_settings.has_key("z") or new_settings.has_key("rz"):
                        os.system("beep -f 6000 -l 15&")


                # Get error from current position to goal if possible
                [px,py,alt,rz] = self.getErrorToGoal()

                # Starting control mode
                if hover and not self.prev_msg.hover:
                    # Starting Goal should be from current position
                    if self.PIDSetCurrentAuto:
                        new_settings["SetCurrent"] = True

                    # Reset PID controllers
                    for p in range(4):
                        self.PID[p].reset(rtime)

                    # Set yaw difference as I part
                    #self.PID[3].error_sum = 0






                if hover:
                    msg.hover = False #TODO should be an option

                    pidmsg = PidMSG()
                    pidmsg.header.stamp = rtime


                    # Get PID control

                    #XY control depends on the asin of the distance
                    #cr, crp, cri, crd = self.PID[0].get_command(-py,rtime,f=lambda d: degrees(asin(d))/90.*self.max_angle)
                    #cp, cpp, cpi, cpd  = self.PID[1].get_command(px,rtime,f=lambda d: degrees(asin(d))/90.*self.max_angle)



                    #cr, crp, cri, crd = self.PID[0].get_command(-py,rtime)
                    #cp, cpp, cpi, cpd  = self.PID[1].get_command(px,rtime)


                    # Distance to angle (X direction = pitch, Y direction = roll)
                    cr, crp, cri, crd = self.PID[0].get_command(-py,rtime,f=self.distFunc )
                    cp, cpp, cpi, cpd  = self.PID[1].get_command(px,rtime,f=self.distFunc )

                    # THRUST
                    ct, ctp, cti, ctd  = self.PID[2].get_command(alt,rtime)
                    # YAW VEL
                    cy, cyp, cyi, cyd  = self.PID[3].get_command(-rz,rtime)#,d=rz,i=copysign(1,-rz)) #TODO: normalise rotation



                    # ROll/Pitch
                    if self.PIDActive[0]:
                        msg.roll = cr + self.trim_roll
                        msg.pitch = cp + self.trim_pitch

                        pidmsg.ypid = [cr, crp, cri, -crd, -py]
                        pidmsg.xpid = [cp, cpp, cpi, -cpd, px]



                    # THRUST
                    if self.PIDActive[1]:

                        # Add base thrust
                        ct += self.thrust[2]

                        # Add roll compensation
                        # TODO: done on the flie itself now @ 250hz


                        # Bound
                        ct = max(self.thrust[0],ct)
                        ct = min(self.thrust[1],ct)

                        # ct in thrust percent
                        msg.thrust = percentageToThrust(ct)
                        pidmsg.zpid= [ct, ctp, cti, ctd, alt]


                    # YAW
                    if self.PIDActive[2]:
                        msg.yaw = cy
                        pidmsg.rzpid= [cy, cyp, cyi, cyd, -rz]



                    self.pub_PID.publish(pidmsg)



                if self.pressed(Button.Cross):
                    new_settings["SetCurrent"] = True


                q = tf.transformations.quaternion_from_euler(radians(msg.roll-self.trim_roll), radians(msg.pitch - self.trim_pitch), radians(-msg.yaw))
                self.pub_tf.sendTransform((0,0,0), q,  rospy.Time.now(), "/cmd", "/cf_gt2d")





            except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):

                if time.time() - self.warntime>2:
                    rospy.logwarn('Could not look up cf_gt2d -> goal')
                    self.warntime = time.time()


        global tid
        if self.pressed(Button.Circle):

            tid = (tid +1)%len(trajectory)
            new_settings["x"], new_settings["y"], new_settings["z"], new_settings["rz"] = trajectory[tid]

        # previous
        if self.pressed(Button.Triangle):
            tid = (tid -1)%len(trajectory)
            new_settings["x"], new_settings["y"], new_settings["z"], new_settings["rz"] = trajectory[tid]


        # Set trim to current input
        if self.released(Button.R1):
            new_settings["trim_roll"] = min(20, max(msg.roll, -20))
            new_settings["trim_pitch"] = min(20, max(msg.pitch, -20))
            rospy.loginfo("Trim updated Roll/Pitch: %r/%r", round(new_settings["trim_roll"],2), round(new_settings["trim_pitch"],2))

        # Reset Trim
        if self.released(Button.Square):
            new_settings["trim_roll"] = 0.0
            new_settings["trim_pitch"] = 0.0
            rospy.loginfo("Pitch reset to 0/0")


        if new_settings != {} and self.dynserver is not None:
            if self.USB:
                new_settings = numpy2python(new_settings)
            self.dynserver.update_configuration(new_settings)
        
        
        # Cache prev joy command

        self.pub_cfJoy.publish(msg)


        self.prev_cmd = self.curr_cmd
        self.prev_msg = msg
        self.prev_msg.hover = hover
Esempio n. 2
0
    def new_joydata(self, joymsg):
        global Button
        global Axes
        #Should run at 100hz. Use launch files roslaunch crazyflie_ros joy.launch
        if self.prev_cmd == None:
            self.prev_cmd = joymsg
            print len(joymsg.axes)
            
            #USB mode
            if len(joymsg.axes) == 27:
                Button = enum(L1=10,R1=11,Select=0,Start=3,L2=8,R2=9,Up=4,Right=5,Down=6,Left=7,Square=15,Cross=14,Triangle=12,Circle=13)
                Axes = enum(SLL=0,SLU=1,SRL=2,SRU=3,Up=4,Right=5,Down=6,Left=7,L2=4+8,R2=4+9,L1=4+10,R1=4+11,Triangle=4+12,Circle=4+13,Cross=4+14,Square=4+15,AccL=16,AccF=17,AccU=18,GyroY=19)   
            return 
        
        self.curr_cmd = joymsg
        hover = False
        
        
#        print "AXES"
#        for i,x in enumerate(joymsg.axes):
#            print " ",i,":",x
#        print "BUTTONS
#        for i,x in enumerate(joymsg.buttons):
#            print " ",i,":",x
            
        x = 0
        y = 0
        z = 0
        r = 0
        r2 = 0
        # Get stick positions [-1 1]
        if self.curr_cmd.buttons[Button.L1]:         
            x = self.joy_scale[0] * self.curr_cmd.axes[Axes.SLL] # Roll
            y = self.joy_scale[1] * self.curr_cmd.axes[Axes.SLU] # Pitch
            r = self.joy_scale[2] * self.curr_cmd.axes[Axes.SRL] # Yaw
            z = self.joy_scale[3] * self.curr_cmd.axes[Axes.SRU] # Thrust            
            r2 = self.joy_scale[4] * (self.curr_cmd.axes[Axes.L2] - self.curr_cmd.axes[Axes.R2])
            hover = self.curr_cmd.axes[Axes.L1]<-0.75
        
        roll = x * self.max_angle
        pitch = y * self.max_angle        
        yaw = 0
        
        
        #TODO use dyn reconf to decide which to use
        if r2!=0:
            yaw = r2 * self.max_yawangle
        else:
            if self.yaw_joy:
                # Deadzone     
                if r < -0.2 or r > 0.2:
                    if r < 0:
                        yaw = (r + 0.2) * self.max_yawangle * 1.25
                    else:
                        yaw = (r - 0.2) * self.max_yawangle * 1.25
                
                
        if hover:
            thrust = int(round(deadband(z,0.2)*32767 + 32767)) #Convert to uint16
        else:    
            thrust = self.thurstToRaw(z)  
            
        trimmed_roll = roll + self.trim_roll
        trimmed_pitch = pitch + self.trim_pitch
        
            
        # Control trim manually        
        new_settings = {}       
        if self.curr_cmd.buttons[Button.Left]:
            new_settings["trim_roll"] = max(self.trim_roll + self.curr_cmd.axes[Axes.Left]/10.0, -10)                         
        if self.curr_cmd.buttons[Button.Right]:
            new_settings["trim_roll"] =  min(self.trim_roll - self.curr_cmd.axes[Axes.Right]/10.0, 10)  
        if self.curr_cmd.buttons[Button.Down]:
            new_settings["trim_pitch"] = max(self.trim_pitch + self.curr_cmd.axes[Axes.Down]/10.0, -10)                
        if self.curr_cmd.buttons[Button.Up]:
            new_settings["trim_pitch"] = min(self.trim_pitch - self.curr_cmd.axes[Axes.Up]/10.0, 10)
        
        # Set trim to current input
        if self.released(Button.R1):
            new_settings["trim_roll"] = min(10, max(trimmed_roll, -10))
            new_settings["trim_pitch"] = min(10, max(trimmed_pitch, -10))   
            rospy.loginfo("Trim updated Roll/Pitch: %r/%r", round(new_settings["trim_roll"],2), round(new_settings["trim_roll"],2))
        
        # Reset Trim
        if self.released(Button.Square):
            new_settings["trim_roll"] = 0
            new_settings["trim_pitch"] = 0       
            rospy.loginfo("Pitch reset to 0/0")        
            
        if new_settings != {} and self.dynserver!=None:
            self.dynserver.update_configuration(new_settings)            
       
        
        
        # TODO: new firmware doesnt need hover_change, it just uses thrust directly
        #(trimmed_roll,trimmed_pitch,yaw,thrust,hover,hover_set, z)
        msg = CFJoyMSG()
        msg.header.stamp = rospy.Time.now()
        msg.roll = trimmed_roll
        msg.pitch = trimmed_pitch
        msg.yaw = yaw
        msg.thrust = thrust
        msg.hover = hover
        #msg.calib = self.pressed(Button.Triangle)
        self.pub_cfJoy.publish(msg)       
        
        
        # Cache prev joy command
        self.prev_cmd = self.curr_cmd