drive changes

Factory_Automation/Drone/app.py

@@ -43,8 +43,17 @@ driveModule.enableDrive()
 while(1) :
     #try to read a new command from bluetooth
     try :
-        imcoming_data = DCE.get_enum_by_name(bluetoothHandle.data_queue[0])
-        driveModule.updateTarget(incoming_data)
+        imcoming_data = DCE.get_enum_by_name(bluetoothHandle.command_queue.pop())
+        if(imcoming_data == DCE.Move)
+            x = bluetoothHandle.data_queue.pop()
+            y = bluetoothHandle.data_queue.pop()
+            print("Received x coordinate: ", x)
+            print("Received x coordinate: ", y)
+            driveModule.updateTarget([x,y])
+        elif(imcoming_data == DCE.Stop) # Stop drone
+        elif(imcoming_data == DCE.Get_Location)
+            #send current position along bluetooth
+            bluetoothHandle.send_command(driveModule.position)
     except:
         pass
 

Factory_Automation/Drone/drivemodule/main.py

-from drivecontrol import drivecontrol
+from motordriver import driver
 import numpy as np
+import time
 
 #test
-drivemodule = drivecontrol()
+motorDriver = driver()
 
-target = np.array([np.uint8(10), np.uint8(20)])
-position = np.array([np.uint8(20), np.uint8(0)])
+motorDriver.setupMotors()
 
+motorDriver.enableMotors()
 
+while(1) :
+    motorDriver.writeLeftPWM(0.75)
+    motorDriver.writeRightPWM(0.75)
+    time.sleep(5)
+
+    motorDriver.writeLeftPWM(0.75)
+    motorDriver.writeRightPWM(0.25)
+    time.sleep(5)
+    
+    motorDriver.writeLeftPWM(0.25)
+    motorDriver.writeRightPWM(0.25)
+    time.sleep(5)
+    
+    motorDriver.writeLeftPWM(0.25)
+    motorDriver.writeRightPWM(0.75)
+    time.sleep(5)
+    
\ No newline at end of file

Factory_Automation/Drone/drivemodule/motordriver.py

 import RPi.GPIO as GPIO
 
-LEFT_MOTOR_EN = 0
-RIGHT_MOTOR_EN = 1
+LEFT_MOTOR_EN = 2
+RIGHT_MOTOR_EN = 3
 
-LEFT_MOTOR_LPWM = 3
-LEFT_MOTOR_RPWM = 4
+LEFT_MOTOR_LPWM = 21
+LEFT_MOTOR_RPWM = 20
 
-RIGHT_MOTOR_LPWM = 5
-RIGHT_MOTOR_RPWM = 6
+RIGHT_MOTOR_LPWM = 16
+RIGHT_MOTOR_RPWM = 12
 
 class driver:
-    LM_LPWM = GPIO.PWM(LEFT_MOTOR_LPWM, 8000)
-    LM_RPWM = GPIO.PWM(LEFT_MOTOR_RPWM, 8000)
-    RM_LPWM = GPIO.PWM(RIGHT_MOTOR_LPWM, 8000)
-    RM_RPWM = GPIO.PWM(RIGHT_MOTOR_RPWM, 8000)
-
+    
     def setupMotors(self):
+        GPIO.cleanup()
+        GPIO.setmode(GPIO.BCM)
+
+        
         GPIO.setup(LEFT_MOTOR_EN, GPIO.OUT)
         GPIO.setup(RIGHT_MOTOR_EN, GPIO.OUT)
         
         GPIO.setup(LEFT_MOTOR_LPWM, GPIO.OUT)
         GPIO.setup(LEFT_MOTOR_RPWM, GPIO.OUT)
+        
+        self.LM_LPWM = GPIO.PWM(LEFT_MOTOR_LPWM, 8000)
+        self.LM_RPWM = GPIO.PWM(LEFT_MOTOR_RPWM, 8000)
         self.LM_LPWM.start(0)
         self.LM_RPWM.start(0)
         
         GPIO.setup(RIGHT_MOTOR_LPWM, GPIO.OUT)
         GPIO.setup(RIGHT_MOTOR_RPWM, GPIO.OUT)
+        self.RM_LPWM = GPIO.PWM(RIGHT_MOTOR_LPWM, 8000)
+        self.RM_RPWM = GPIO.PWM(RIGHT_MOTOR_RPWM, 8000)
         self.RM_LPWM.start(0)
         self.RM_RPWM.start(0)
         
-    def enableMotors() :
+    def enableMotors(self) :
         GPIO.output(LEFT_MOTOR_EN, GPIO.HIGH)
         GPIO.output(RIGHT_MOTOR_EN, GPIO.HIGH)
         
-    def disableMotors() :
+    def disableMotors(self) :
         GPIO.output(LEFT_MOTOR_EN, GPIO.LOW)
         GPIO.output(RIGHT_MOTOR_EN, GPIO.LOW)
         
@@ -43,7 +48,7 @@ class driver:
             self.LM_LPWM.ChangeDutyCycle(0)
             self.LM_RPWM.ChangeDutyCycle((dutycycle - 0.5)*100)
         elif dutycycle < 0.5 :
-            self.LM_LPWM.ChangeDutyCycle((dutycycle - 0.5)*100)
+            self.LM_LPWM.ChangeDutyCycle((0.5 - dutycycle)*100)
             self.LM_RPWM.ChangeDutyCycle(0)
         else :
             self.LM_LPWM.ChangeDutyCycle(0)
@@ -55,7 +60,7 @@ class driver:
             self.RM_LPWM.ChangeDutyCycle(0)
             self.RM_RPWM.ChangeDutyCycle((dutycycle - 0.5)*100)
         elif dutycycle < 0.5 :
-            self.RM_LPWM.ChangeDutyCycle((dutycycle - 0.5)*100)
+            self.RM_LPWM.ChangeDutyCycle((0.5 - dutycycle)*100)
             self.RM_RPWM.ChangeDutyCycle(0)
         else :
             self.RM_LPWM.ChangeDutyCycle(0)