deleting old files

servo_lcd/src/servo_check.py

@@ -1,28 +1,113 @@
+#!/usr/bin/python3
 
 from gpiozero import Servo
 from time import sleep
 from gpiozero.pins.pigpio import PiGPIOFactory
 from math import *
+import time
+from board import SCL,SDA
+import busio
+from adafruit_extended_bus import ExtendedI2C as I2C
+from adafruit_pca9685 import PCA9685,PWMChannel
+from adafruit_servokit import ServoKit
+
+
+i2c1=busio.I2C(SCL,SDA)
+i2c2=busio.I2C(SCL,SDA)
 
 class SpotServoCheck:
 
-    def __init__(self,pin_value):
-        self.factory=PiGPIOFactory()
-        self.min_pulse=0.25/1000
-        self.max_pulse=2.75/1000
-        self.pin=pin_value
-        self.servo=Servo(self.pin,min_pulse_width=self.min_pulse,max_pulse_width=self.max_pulse,pin_factory=self.factory)
+    def __init__(self):
+        #self.factory=PiGPIOFactory()
+        self.min_pulse=580
+        self.max_pulse=2350
+
+        self.kit=ServoKit(channels=16,i2c=i2c1)
+        self.kit2=ServoKit(channels=16,i2c=i2c2)
+        #self.servo=Servo(,min_pulse_width=self.min_pulse,max_pulse_width=self.max_pulse,pin_factory=self.factory)
         self.MAX_SERVO_ANGLE=84
+
+
+
+
+    def STANCE(self):
+        #BACK LEFT
+        self.kit.servo[0].angle=135
+        self.kit.servo[1].angle=15
+        self.kit.servo[2].angle=0
+        #FRONT LEFT
+        self.kit.servo[4].angle=40
+        self.kit.servo[5].angle=42
+        self.kit.servo[6].angle=10
+
+         #BACK RIGHT
+        self.kit.servo[8].angle=130
+        self.kit.servo[9].angle=0
+        self.kit.servo[10].angle=100
+
+        #FRONT RIGHT
+        self.kit.servo[12].angle=10
+        self.kit.servo[13].angle=80
+
+        self.kit.servo[14].angle=20
+
+    def SLEEP(self):
+        #FRONT LEFT
+        self.kit.servo[0].angle=90
+        self.kit.servo[1].angle=0
+        self.kit.servo[2].angle=0
+        #BACK LEFT
+        self.kit.servo[4].angle=10
+        self.kit.servo[5].angle=0
+        self.kit.servo[6].angle=0
+      #BACK RIGHT
+        self.kit.servo[8].angle=180
+        self.kit.servo[9].angle=0
+        self.kit.servo[10].angle=100
+
+        #FRONT RIGHT
+        self.kit.servo[12].angle=50
+        self.kit.servo[13].angle=110
+
+        self.kit.servo[14].angle=20
+
+
+
+        #self.kit.servo[1].angle=0
+
+
+    def test(self):
+
+
+        #Shoulder
+        self.kit.servo[2].angle=0
+        self.kit.servo[6].angle=0
+        self.kit.servo[10].angle=0
+        self.kit.servo[14].angle=0
+
+         #Knee
+        self.kit.servo[0].angle=90
+        self.kit.servo[4].angle=0
+        self.kit.servo[8].angle=160
+        self.kit.servo[12].angle=80
 
-    def run(self,n=0):
-        print("CHECKING THE SERVO...{}".format(self.pin))
-        while n<10:
-            for i in range(0,180):
-                self.servo.value=sin(radians(i))
-                sleep(0.01)
-            n+=1
-        print("SERVO WORKS GOOD")       
+        #HIP
+
+        self.kit.servo[1].angle=0
+        self.kit.servo[5].angle=0
+        self.kit.servo[9].angle=60
+        self.kit.servo[13].angle=110
 
 if __name__=="__main__":
-    hip=SpotServoCheck(21)
-    hip.run()
+    hip=SpotServoCheck()
+
+    ''' hip.SLEEP()
+    time.sleep(5)
+    hip.STANCE()
+    time.sleep(8)
+    hip.SLEEP()'''
+    '''time.sleep(3)'''
+    #hip.STANCE()
+    hip.SLEEP()
+   # hip.test()
+

servo_lcd/src/servo_pwm.py

@@ -0,0 +1,27 @@
+#/usr/bin/python3
+from adafruit_pca9685 import PCA9685
+from adafruit_motor import servo
+from board import SCL,SDA
+import busio
+import time
+
+i2c=busio.I2C(SCL,SDA)
+
+pca_1 = PCA9685(i2c, address=0x40)
+pca_1.frequency = 60
+servo2 = servo.Servo(pca_1.channels[0], min_pulse=460, max_pulse=2450)
+
+
+
+sweep = range(1, int(180), +1)
+sweep_backward = range(int(180), 1, -1)
+while True:
+    for degree in sweep :
+        print(degree)
+        servo2.angle = int(degree)
+        time.sleep(0.01)
+    for degree in sweep_backward:
+        print(degree)
+        servo2.angle = int(degree)
+        print( servo2.angle )
+        time.sleep(0.01)

spot_simulation/SleepMode.py

@@ -0,0 +1,70 @@
+#!/usr/bin/python3
+import rospy
+from spot_simulation.msg import JointAngles
+import enum
+from std_msgs.msg import String
+from spot import SpotBehaviour
+from adafruit_servokit import ServoKit 
+import busio
+from board import SCL,SDA
+
+class Sleep:
+
+    def __init__(self,calibration_mode):
+        I2C=busio.I2C(SCL,SDA)
+        self.calibrate=calibration_mode
+        self.state=SpotBehaviour.REST
+        self.joint_angles_pub=rospy.Publisher('/joint_state',JointAngles,queue_size=10)
+        self.rate=rospy.Rate(1)
+        self.kit=ServoKit(channels=16,i2c=I2C)
+
+        self.robot={"Front Right":[],
+                    "Back  Right":[],
+                    "Front Left":[],
+                    "Back  Left":[]}
+        self.jointstate=JointAngles()
+
+    def rest(self):
+        #FRONT LEFT
+        self.kit.servo[0].angle=90
+        self.kit.servo[1].angle=0
+        self.kit.servo[2].angle=0
+        self.robot["Front Left"]=[90,0,0]
+
+        #BACK LEFT
+        self.kit.servo[4].angle=10
+        self.kit.servo[5].angle=0
+        self.kit.servo[6].angle=0
+        self.robot["Back Left"]=[10,0,0]
+
+        #FRONT RIGHT
+        self.kit.servo[8].angle=180
+        self.kit.servo[9].angle=0
+        self.kit.servo[10].angle=100
+        self.robot["Front Right"]=[180,0,100]
+
+        #BACK RIGHT
+        self.kit.servo[12].angle=50
+        self.kit.servo[13].angle=110
+        self.kit.servo[14].angle=20
+        self.robot["Back Right"]=[50,110,20]
+
+        self.jointstate.front_right=self.robot["Front Right"]
+        self.jointstate.front_left=self.robot["Front Left"]
+        self.jointstate.back_right=self.robot["Back Right"]
+        self.jointstate.back_left=self.robot["Back Left"] 
+
+        while not rospy.is_shutdown():
+
+            self.joint_angles_pub.publish(self.jointstate)
+            rospy.loginfo(self.jointstate)
+            self.rate.sleep()
+
+
+if __name__ =="__main__":
+    rospy.init_node("sleep node")
+    robot=Sleep(True)
+    robot.rest()
+
+
+

spot_simulation/launch/Robotcontroller.launch

@@ -0,0 +1,11 @@
+<launch>
+
+
+    <node name="body" pkg="spot_simulation" type="RobotControl.py" output="screen"/>
+
+
+
+
+    <rosparam command="load" file="$(find spot_simulation)/config/dimension.yaml" />
+
+</launch>

spot_simulation/launch/imu.launch

@@ -0,0 +1,8 @@
+<launch>
+
+   <!--THIS LAUNCH FILE IS TO RUN THE IMU NODE TO GET IMU TOPIC-->
+    <node name="imu" pkg="spot_simulation" type="imu.py" output="screen"/>
+    <node name="imu_tf" pkg="spot_simulation" type="tf_imu.py"/>
+
+
+</launch>

spot_simulation/launch/kalman_filter.launch

@@ -0,0 +1,13 @@
+<launch>
+  <node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf">
+    <param name="output_frame" value="odom"/>
+
+    <param name="freq" value="35.0"/>
+    <param name="sensor_timeout" value="0.75"/>
+    <param name="odom_used" value="false"/>
+    <param name="imu_used" value="true"/>
+    <param name="vo_used" value="false"/>
+    <param name="debug" value="false"/>
+    <param name="self_diagnose" value="true"/>
+  </node>
+ </launch>

spot_simulation/launch/robot.launch

@@ -0,0 +1,7 @@
+<launch>
+
+    <node name="controller" pkg="spot_simulation" type="controller.py" output="screen" required="true"/>
+
+    <rosparam command="load" file="$(find spot_simulation)/config/dimension.yaml" />
+
+</launch>

spot_simulation/launch/sleepstate.launch

@@ -0,0 +1,7 @@
+<launch>
+
+
+    <node name="Sleep" pkg="spot_simulation" type="SleepMode.py" />
+
+
+</launch>

spot_simulation/launch/standstate.launch

@@ -0,0 +1,7 @@
+<launch>
+
+
+    <node name="Stand" pkg="spot_simulation" type="StandMode.py" output="screen"/>
+
+
+</launch>

spot_simulation/scripts/RobotControl.py

@@ -0,0 +1,53 @@
+#!/usr/bin/python3
+import os
+from std_msgs.msg import Int64,Bool
+import rospy
+from spot import SpotBehaviour
+from SleepMode import Sleep
+from StandMode import Stance
+
+subscribed_state=3
+class Ramu:
+
+    def __init__(self,calibrate):
+
+        self.calibration_mode=False
+        self.default_state=SpotBehaviour.REST
+
+        self.sleep=True
+        self.Stand=True
+        self.crawl=True
+
+    def state_callback(self,msg):
+        robot_state=msg.data
+        subscribed_state=robot_state
+
+        #rospy.loginfo(subscribed_state)
+        while True:
+            #rospy.loginfo(self.subscribed_state)
+            if subscribed_state==0:
+                Sleep(False).rest()
+                rospy.loginfo("RESTING")
+            elif subscribed_state==3:
+                Stance(False).stand()
+                rospy.loginfo("STAND")
+
+
+    def state_pubilsher(self):
+
+        #rospy.loginfo(self.subscribed_state)
+        while not rospy.is_shutdown():
+            #rospy.loginfo(self.subscribed_state)
+            if subscribed_state==0:
+                Sleep(False).rest()
+                rospy.loginfo("RESTING")
+            elif subscribed_state==3:
+                Stance(False).stand()
+                rospy.loginfo("STAND")
+
+if __name__=="__main__":
+
+    spot=Ramu(True)
+    rospy.init_node('robot_control')
+    rospy.Subscriber('/spot_state',Int64,spot.state_callback)
+    spot.state_pubilsher()