mpu wip

Author: previ

coderbot.py

@@ -27,28 +27,31 @@
 
 # GPIO
 # motors
-PIN_MOTOR_ENABLE = 22
-PIN_LEFT_FORWARD = 25
-PIN_LEFT_BACKWARD = 24
-PIN_RIGHT_FORWARD = 4
-PIN_RIGHT_BACKWARD = 17
+PIN_MOTOR_ENABLE = None #22
+PIN_LEFT_FORWARD = 17 #25
+PIN_LEFT_BACKWARD = 18 # 24
+PIN_RIGHT_FORWARD = 22 # 4
+PIN_RIGHT_BACKWARD = 23 #17
 #?
-PIN_PUSHBUTTON = 11
+PIN_PUSHBUTTON = 16 #11
 # servo
-PIN_SERVO_3 = 9
-PIN_SERVO_4 = 10
+PIN_SERVO_3 = 7 #9
+PIN_SERVO_4 = 1 #10
 # sonar
-PIN_SONAR_1_TRIGGER = 18
-PIN_SONAR_1_ECHO = 7
-PIN_SONAR_2_TRIGGER = 18
-PIN_SONAR_2_ECHO = 8
-PIN_SONAR_3_TRIGGER = 18
-PIN_SONAR_3_ECHO = 23
+PIN_SONAR_1_TRIGGER = 5 #18
+PIN_SONAR_1_ECHO = 27 #7
+PIN_SONAR_2_TRIGGER = 5 #18
+PIN_SONAR_2_ECHO = 6 #8
+PIN_SONAR_3_TRIGGER = 5 #18
+PIN_SONAR_3_ECHO = 12 #23
+PIN_SONAR_4_TRIGGER = 5 #18
+PIN_SONAR_4_ECHO = 13 #23
+
 # encoder
 PIN_ENCODER_LEFT_A = 14
-PIN_ENCODER_LEFT_B = 6
-PIN_ENCODER_RIGHT_A = 15
-PIN_ENCODER_RIGHT_B = 12
+PIN_ENCODER_LEFT_B = 15 #6
+PIN_ENCODER_RIGHT_A = 24 #15
+PIN_ENCODER_RIGHT_B = 25 #12
 
 # PWM
 PWM_FREQUENCY = 100 #Hz
@@ -58,7 +61,7 @@ class CoderBot(object):
 
     # pylint: disable=too-many-instance-attributes
 
-    _pin_out = [PIN_MOTOR_ENABLE, PIN_LEFT_FORWARD, PIN_RIGHT_FORWARD, PIN_LEFT_BACKWARD, PIN_RIGHT_BACKWARD, PIN_SERVO_3, PIN_SERVO_4]
+    _pin_out = [PIN_LEFT_FORWARD, PIN_RIGHT_FORWARD, PIN_LEFT_BACKWARD, PIN_RIGHT_BACKWARD, PIN_SERVO_3, PIN_SERVO_4]
 
     def __init__(self, motor_trim_factor=1.0, encoder=True):
         self.pi = pigpio.pi('localhost')
@@ -89,7 +92,8 @@ def __init__(self, motor_trim_factor=1.0, encoder=True):
 
         self.sonar = [sonar.Sonar(self.pi, PIN_SONAR_1_TRIGGER, PIN_SONAR_1_ECHO),
                       sonar.Sonar(self.pi, PIN_SONAR_2_TRIGGER, PIN_SONAR_2_ECHO),
-                      sonar.Sonar(self.pi, PIN_SONAR_3_TRIGGER, PIN_SONAR_3_ECHO)]
+                      sonar.Sonar(self.pi, PIN_SONAR_3_TRIGGER, PIN_SONAR_3_ECHO),
+                      sonar.Sonar(self.pi, PIN_SONAR_4_TRIGGER, PIN_SONAR_4_ECHO)]
 
         try:
             self._ag = mpu.AccelGyro()
@@ -126,11 +130,11 @@ def turn(self, speed=100, elapse=0, steps=-1):
         self.motor_control(speed_left=speed_left, speed_right=speed_right, time_elapse=elapse)
 
     def turn_angle(self, speed=100, angle=0):
-        z = self._ag.get_gyro_data()['z']
+        z = self._ag.get_gyro()[2]
         self.turn(speed, elapse=0)
-        while abs(z - self._ag.get_gyro_data()['z']) < angle:
+        while abs(z - self._ag.get_gyro()[2]) < angle:
             time.sleep(0.05)
-            logging.info(self._ag.get_gyro_data()['z'])
+            logging.info(self._ag.get_gyro()[2])
         self.stop()
 
     def forward(self, speed=100, elapse=0, distance=0):
@@ -145,9 +149,22 @@ def left(self, speed=100, elapse=0):
     def right(self, speed=100, elapse=0):
         self.turn(speed=speed, elapse=elapse)
 
+    def servo3(self, angle):
+        self._servo_control(PIN_SERVO_3, angle)
+
+    def servo4(self, angle):
+        self._servo_control(PIN_SERVO_4, angle)
+
     def get_sonar_distance(self, sonar_id=0):
         return self.sonar[sonar_id].get_distance()
 
+    def _servo_control(self, pin, angle):
+        duty = ((angle + 90) * 100 / 180) + 25
+
+        self.pi.set_PWM_range(pin, 1000)
+        self.pi.set_PWM_frequency(pin, 50)
+        self.pi.set_PWM_dutycycle(pin, duty)
+
     def stop(self):
         if self._encoder:
             self._twin_motors_enc.stop()

lsm9ds1.py

@@ -297,7 +297,7 @@ def read_values(self):
         tempf = (tempc * 9/5) + 32
         acc = [c * lsm9ds1.ACC_SENSOR_SCALE for c in acc]
         gyro = [g * lsm9ds1.DPS_SENSOR_SCALE for g in gyro]
-        return tempf, acc, gyro
+        return tempc, acc, gyro
 
     #
     # Raw interface

mpu.py

@@ -1,20 +1,58 @@
-import MPU6050
+import lsm9ds1
 import time
 
 class AccelGyro:
-    GYRO_THRESHOLD = 1.5
+    X_IND = 0
+    Y_IND = 1
+    Z_IND = 2
+
+    PITCH_IND = 0
+    ROLL_IND = 1
+    YAW_IND = 2
 
+    """This example shows how to poll the sensor for new data.
+    It queries the sensor to discover when the accelerometer/gyro
+    has new data and then reads all the sensors."""
     def __init__(self):
-        self.ag = MPU6050.MPU6050(0x68)
-        self.ag.set_gyro_range(MPU6050.MPU6050.GYRO_RANGE_250DEG)
-        self.gyro_abs = {'x': 0.0, 'y': 0.0, 'z': 0.0}
-        self.t = time.time()
-
-    def get_gyro_data(self):
-        gyro_data = self.ag.get_gyro_data()
-        for k in gyro_data.keys():
-            if abs(gyro_data[k]) > self.GYRO_THRESHOLD:
-                self.gyro_abs[k] += gyro_data[k] * (time.time() - self.t)
-
-        self.t = time.time()
-        return self.gyro_abs
+        self.driver = lsm9ds1.make_i2c(1)
+        mc = lsm9ds1.MagCalibration(xmin=-0.3612, xmax=-0.17836000000000002,
+                                    ymin=-0.08750000000000001, ymax=0.07826000000000001,
+                                    heading_offset=95.3491645593403)
+        self.driver.configure(mc)
+
+
+    GYRO_THRESHOLD = 1.5
+
+    def read_ag(self):
+        temp, acc, gyro = self.driver.read_values()
+        print("Temp: %.1f °f" % temp)
+        print("Gyro Roll: %.4f, Pitch: %.4f, Yaw: %.4f" % (gyro[SimpleExample.ROLL_IND],
+                                                           gyro[SimpleExample.PITCH_IND],
+                                                           gyro[SimpleExample.YAW_IND]))
+        print("X: %.4f, Y: %.4f, Z: %.4f" % (acc[SimpleExample.X_IND],
+                                             acc[SimpleExample.Y_IND],
+                                             acc[SimpleExample.Z_IND]))
+
+    def read_magnetometer(self):
+        hdg = self.driver.mag_heading()
+        print("Heading: %.2f" % hdg)
+
+
+    def get_gyro(self):
+        temp, acc, gyro = self.driver.read_values()
+        return (gyro[SimpleExample.ROLL_IND],
+                gyro[SimpleExample.PITCH_IND],
+                gyro[SimpleExample.YAW_IND])
+
+    def get_acc(self):
+        temp, acc, gyro = self.driver.read_values()
+        return (gyro[SimpleExample.X_IND],
+                gyro[SimpleExample.Y_IND],
+                gyro[SimpleExample.Z_IND])
+
+    def get_hdg(self):
+        hdg = self.driver.mag_heading()
+
+    def get_temp(self):
+        temp, acc, gyro = self.driver.read_values()
+        return temp

mpu2.py

@@ -3,19 +3,19 @@
 import lsm9ds1
 
 class SimpleExample:
-    X_IND = 1
-    Y_IND = 2
-    Z_IND = 0
+    X_IND = 0
+    Y_IND = 1
+    Z_IND = 2
 
-    PITCH_IND = 1
-    ROLL_IND = 0
+    PITCH_IND = 0
+    ROLL_IND = 1
     YAW_IND = 2
 
     """This example shows how to poll the sensor for new data.
     It queries the sensor to discover when the accelerometer/gyro
     has new data and then reads all the sensors."""
     def __init__(self):
-        self.driver = lsm9ds1.make_i2c(0)
+        self.driver = lsm9ds1.make_i2c(1)
         mc = lsm9ds1.MagCalibration(xmin=-0.3612, xmax=-0.17836000000000002,
                                     ymin=-0.08750000000000001, ymax=0.07826000000000001,
                                     heading_offset=95.3491645593403)

rotary_encoder/motorencoder.py

@@ -81,7 +81,8 @@ def control(self, power=100.0, time_elapse=0):
         self._direction = 1 if power > 0 else -1  # setting direction according to speed
         self._power = power # setting current power
 
-        self._pi.write(self._enable_pin, True)  # enabling motors
+        if self._enable_pin is not None:
+            self._pi.write(self._enable_pin, True)  # enabling motors
 
         # going forward
         if (self._direction == 1):

rotary_encoder/wheelsaxel.py

@@ -30,10 +30,10 @@ def __init__(self, pi, enable_pin,
         # right motor
         self._right_motor = MotorEncoder(pi,
                                          enable_pin,
-                                         right_forward_pin,
                                          right_backward_pin,
-                                         right_encoder_feedback_pin_B,
-                                         right_encoder_feedback_pin_A)
+                                         right_forward_pin,
+                                         right_encoder_feedback_pin_A,
+                                         right_encoder_feedback_pin_B)
 
         # other
         self._wheelsAxle_lock = threading.Condition() # race condition lock
@@ -86,7 +86,7 @@ def control_time(self, power_left=100, power_right=100, time_elapse=0):
         self._is_moving = True
 
         # moving for desired time
-        sleep(time_elapse)
+        sleep(max(time_elapse, 0))
         self.stop()
 
     """ Motor distance control allows the motors

rotary_encoder_tests/rotary_encoder_coderbot_test.py

@@ -4,5 +4,5 @@
 
 c._twin_motors_enc.control_distance(100, 100, 200)
 
-while(True):
-    print(c._twin_motors_enc.distance())
+#while(True):
+print(c._twin_motors_enc.distance())

rotary_encoder_tests/rotary_encoder_raw_test.py

@@ -1,7 +1,7 @@
-PIN_ENCODER_LEFT_A = 14
-PIN_ENCODER_LEFT_B = 6
-PIN_ENCODER_RIGHT_A = 15
-PIN_ENCODER_RIGHT_B = 12
+PIN_ENCODER_LEFT_A = 14 
+PIN_ENCODER_LEFT_B = 15
+PIN_ENCODER_RIGHT_A = 24
+PIN_ENCODER_RIGHT_B = 25
 
 from time import sleep
 import pigpio

static/js/blockly/blocks.js

@@ -386,6 +386,38 @@ Blockly.Python['coderbot_adv_motor'] = function(block) {
   return code;
 };
 
+Blockly.Blocks['coderbot_adv_move_enc'] = {
+  // Block for moving forward.
+  init: function() {
+    this.setHelpUrl('http://code.google.com/p/blockly/wiki/Motor');
+    this.setColour(40);
+    
+    this.appendValueInput('SPEED')
+        .setCheck('Number')
+        .appendField(Blockly.Msg.CODERBOT_MOVE_ADV_MOTOR + " " + Blockly.Msg.CODERBOT_MOVE_ADV_MOTOR_SPEED);
+    this.appendValueInput('DISTANCE')
+        .setCheck('Number')
+        .appendField(Blockly.Msg.CODERBOT_MOVE_ADV_MOTOR_DISTANCE);
+    this.setInputsInline(true);
+    // Assign 'this' to a variable for use in the tooltip closure below.
+    var thisBlock = this;
+    this.setTooltip(function() {
+      var mode = thisBlock.getFieldValue('ACTION');
+      return TOOLTIPS[mode] + Blockly.Msg.CODERBOT_MOVE_ADV_TIP_TAIL;
+    });
+    this.setPreviousStatement(true);
+    this.setNextStatement(true);
+  }
+};
+
+Blockly.Python['coderbot_adv_move_enc'] = function(block) {
+  // Generate Python for moving forward.
+  var speed = Blockly.Python.valueToCode(block, 'SPEED', Blockly.Python.ORDER_NONE);
+  var distance = Blockly.Python.valueToCode(block, 'DISTANCE', Blockly.Python.ORDER_NONE);
+  var code = "get_bot().move(speed=" + speed + ", distance=" + distance + ")\n";
+  return code;
+};
+
 Blockly.Blocks['coderbot_adv_stop'] = {
   // Block to stop the get_bot().
   init: function() {
@@ -1055,7 +1087,8 @@ Blockly.Blocks['coderbot_sonar_get_distance'] = {
         .appendField(Blockly.Msg.CODERBOT_SONAR_GET_DISTANCE)
         .appendField(new Blockly.FieldDropdown([[Blockly.Msg.CODERBOT_SONAR_SENSOR_1, "0"],
                                                 [Blockly.Msg.CODERBOT_SONAR_SENSOR_2, "1"],
-                                                [Blockly.Msg.CODERBOT_SONAR_SENSOR_3, "2"]]), 'SONAR');
+                                                [Blockly.Msg.CODERBOT_SONAR_SENSOR_3, "2"],
+                                                [Blockly.Msg.CODERBOT_SONAR_SENSOR_4, "3"]]), 'SONAR');
     this.setOutput(true, 'Number');
     this.setTooltip(Blockly.Msg.LOGIC_BOOLEAN_TOOLTIP);
   }

static/js/blockly/bot_en.js

@@ -17,6 +17,8 @@ Blockly.Msg.CODERBOT_MOVE_ADV_MOTOR_SPEED_LEFT = "speed left"
 Blockly.Msg.CODERBOT_MOVE_ADV_MOTOR_SPEED_RIGHT = "speed right"
 Blockly.Msg.CODERBOT_MOVE_ADV_MOTOR_STEPS_LEFT = "steps left"
 Blockly.Msg.CODERBOT_MOVE_ADV_MOTOR_STEPS_RIGHT = "steps right"
+Blockly.Msg.CODERBOT_MOVE_ADV_MOTOR_SPEED = "at speed"
+Blockly.Msg.CODERBOT_MOVE_ADV_MOTOR_DISTANCE = "distance"
 Blockly.Msg.CODERBOT_MOVE_ADV_ELAPSE = "for"
 Blockly.Msg.CODERBOT_MOVE_MOTION_DIST = "distance"
 Blockly.Msg.CODERBOT_MOVE_MOTION_ANGLE = "angle"
@@ -80,6 +82,7 @@ Blockly.Msg.CODERBOT_SONAR_GET_DISTANCE = "get distance with sonar";
 Blockly.Msg.CODERBOT_SONAR_SENSOR_1 = "1";
 Blockly.Msg.CODERBOT_SONAR_SENSOR_2 = "2";
 Blockly.Msg.CODERBOT_SONAR_SENSOR_3 = "3";
+Blockly.Msg.CODERBOT_SONAR_SENSOR_4 = "4";
 Blockly.Msg.CODERBOT_EVENT_WHEN = "when";
 Blockly.Msg.CODERBOT_EVENT_WITH = "with";
 Blockly.Msg.CODERBOT_EVENT_PUBLISH = "publish";