Diff: Motors.cpp

Revision:

12:82b8fe254222

Parent:

6:3872858b7844

diff -r 0b9098ec11c7 -r 82b8fe254222 Motors.cpp
--- a/Motors.cpp	Thu Dec 19 00:13:38 2019 +0000
+++ b/Motors.cpp	Sun Jan 05 15:42:22 2020 +0000
@@ -51,27 +51,8 @@
 int previousStateB = currentStateB;
 int nextmoveflagB = 1;
 
-//X is forward, Z is upwards, Y is sideways in this structure.
-//Order is X, Y, Z linear, then X, Y, Z rotational, from 1:6.
-void cmdvelresponse(const std_msgs::Int32MultiArray& vinput) {
-    //Extracting the only commands that will be used in the multiarray, and assuming that the int value attached to each point is 100 times
-    //the intended PWM constant for that movement command (e.g. a value of 50 in msg.data[6] would give a PWM duty cycle of 0.5 for rotation):
-    float xlin = 0.01 * vinput.data[1];
-    float zrot = 0.01 * vinput.data[6];
-    //assume rotation needs to be taken care of first, generally, then linear movement
-    if(zrot != 0) { //assume positive z is clockwise, negative is anticlockwise, A is left, B is right
-        A.speed(-zrot);
-        B.speed(zrot);
-    } else if(xlin != 0) {
-        A.speed(xlin);
-        B.speed(xlin);
-    } else {
-        A.stop(1);
-        B.stop(1);
-    }
-}
-
-void initialize() {
+void initialize()
+{
     //every time a rising or falling edge is detected for the A or B channels, call the encoder counting interrupt for the respective wheel.
     //this corresponds to the counting program for the encoders for each wheel, given that quadrature encoding is used.
     encoderAchannel1.rise(&callbackA);
@@ -93,16 +74,17 @@
     previousStateB = currentStateB;
 }
 
-void callbackA() {
+void callbackA()
+{
     ////////
     //insert the 'if X, carry out the rest of this code' debouncing check here, ensure the delay is minimal enough to avoid hindering 3576 measurements per rotation at max speed
     ////////
-    
+
     volatile int changeA = 0;
     channelAstate1 = encoderAchannel1.read();
     channelAstate2 = encoderAchannel2.read();
     currentStateA = (channelAstate1 << 1) | (channelAstate2);
-    
+
     if (((currentStateA ^ previousStateA) != INVALID) && (currentStateA != previousStateA)) {
         //2 bit state. Right hand bit of prev XOR left hand bit of current
         //gives 0 if clockwise rotation and 1 if counter clockwise rotation.
@@ -122,18 +104,19 @@
     }
 }
 
-void callbackB() {
+void callbackB()
+{
     ////////
     //insert the 'if X, carry out the rest of this code' debouncing check here, ensure the delay is minimal enough to avoid hindering 3576 measurements per rotation at max speed
     ////////
-    
+
     //every time this function is called, increment or decrement the encoder count depending on which direction the relevant wheel is moving
-    
+
     volatile int changeB = 0;
     channelBstate1 = encoderBchannel1.read();
     channelBstate2 = encoderBchannel2.read();
     currentStateB = (channelBstate1 << 1) | (channelBstate2);
-    
+
     if (((currentStateB ^ previousStateB) != INVALID) && (currentStateB != previousStateB)) {
         //2 bit state. Right hand bit of prev XOR left hand bit of current
         //gives 0 if clockwise rotation and 1 if counter clockwise rotation.
@@ -153,7 +136,8 @@
     }
 }
 
-void move(const std_msgs::Int32& dinput) {
+void move(const std_msgs::Int32& dinput)
+{
     if(nextmoveflagA == 1 && nextmoveflagB == 1) {
         //import the relevant ROS message and convert it to a usable encoder target
         float distance = dinput.data;
@@ -168,7 +152,8 @@
     }
 }
 
-void tempMove(float distance) {
+void tempMove(float distance)
+{
     if(nextmoveflagA == 1 && nextmoveflagB == 1) {
         float mtarget = distance/(2*3.142*radius)*benchmark;
         atarget = ceil(mtarget + aprevtarget);
@@ -180,7 +165,8 @@
     }
 }
 
-void rotate(const std_msgs::Int32& rinput) {
+void rotate(const std_msgs::Int32& rinput)
+{
     if(nextmoveflagA == 1 && nextmoveflagB == 1) {
         //import the relevant ROS message and convert it to usable encoder targets
         float degrees = rinput.data;
@@ -195,7 +181,8 @@
     }
 }
 
-void tempRotate(float degrees) {
+void tempRotate(float degrees)
+{
     if(nextmoveflagA == 1 && nextmoveflagB == 1) {
         float rtarget = (degrees/(360*3))*9.15*benchmark;
         atarget = ceil(rtarget + aprevtarget);
@@ -207,7 +194,8 @@
     }
 }
 
-void driveMotors() {
+void driveMotors()
+{
     //depending on the direction the wheels have to move, set their pwms to either positive or negative so each motor moves correctly
     if(atarget > aprevtarget) {
         apwm = pwm;
@@ -241,3 +229,4 @@
     aprevtarget = atarget;
     bprevtarget = btarget;
 }
+