Diff: main.cpp

Revision:

0:fb7201282a86

diff -r 000000000000 -r fb7201282a86 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sun Mar 21 19:54:07 2021 +0000
@@ -0,0 +1,125 @@
+#include "mbed.h"
+#include <Motor.h>
+
+Motor motor0(p5,p6,p7);
+Motor motor1(p8,p11,p12);
+Motor motor2(p13,p14,p15);  //pull,drive,enable
+Motor motor3(p16,p17,p18);
+Motor motor4(p21,p22,p23);
+Motor motor5(p24,p25,p26);
+
+const int HIGH = 1;
+const int LOW  = 0;
+
+//@@@@@@@@@@@@@@@__CONTROL__@@@@@@@@@@@@@@@@@@@
+const int PPR = 400;    //Pulses Per Revolution
+int ROTATION = 36000;   //3600 full rotation
+int RPM = 10;            //Rotations Per Minute
+int MOVEMENT = 0;       //Movement Type |0=F ,1=B ,2=R ,3=L|
+int NUMSTEPS = 10;       //Number of Steps
+//@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@@
+
+int pulseLength = 0;
+
+//Converts desired RPM to required Pulse Length (In Microseconds)
+void Configure_Pulse(float rpm){
+    float holder = 0;           //Create holder variable (MBED is Stupid)
+
+    holder = rpm;               //Set to RPM
+    holder = holder/60;         //Brain.exe is no longer responding ;(
+    holder = holder*PPR;        //Multiply by PPR
+    holder = holder/2;          //Brain.exe is no longer responding ;(
+    holder = 1/holder;          //Convert to Seconds
+    holder = holder*1000000;    //Convert to Microseconds
+    holder = holder/60;         //Dividing by Gear Ratio
+    holder = holder/4;          //Math Magic
+    
+    pulseLength = holder;       
+}
+
+//Sets Drive Pins to Movement Configuration
+void Configure_Movement(int input){
+    switch(input){
+        //Move Forward
+        case 0: motor0.dir = HIGH;
+                motor1.dir = HIGH;
+                motor2.dir = HIGH;
+                motor3.dir = HIGH;
+                motor4.dir = HIGH;
+                motor5.dir = HIGH;
+            break;
+        //Move Backward
+        case 1: motor0.dir = LOW;
+                motor1.dir = LOW;
+                motor2.dir = LOW;
+                motor3.dir = LOW;
+                motor4.dir = LOW;
+                motor5.dir = LOW;
+            break;
+        //Turn Right
+        case 2: motor0.dir = HIGH;
+                motor1.dir = HIGH;
+                motor2.dir = HIGH;
+                motor3.dir = LOW;
+                motor4.dir = LOW;
+                motor5.dir = LOW;
+            break;
+        //Turn Left
+        case 3: motor0.dir = LOW;
+                motor1.dir = LOW;
+                motor2.dir = LOW;
+                motor3.dir = HIGH;
+                motor4.dir = HIGH;
+                motor5.dir = HIGH;
+            break;
+    }
+}
+
+int main() {
+    Configure_Movement(MOVEMENT);
+    Configure_Pulse(RPM);
+    
+    bool altSet = false;    //
+    int inv0 = 0, inv1 = 0; //Creates inverter variables for pulse controll
+    
+    //Loop for Number of Steps Made
+    for(int i=0; i<NUMSTEPS; i++){
+        if(altSet == false){
+            //Pulses for total desired Rotation for set0
+            for(int j=0; j<ROTATION; j++){
+                inv0 =~ inv0;   //Inverts Value
+                motor0.pul = inv0;
+                motor2.pul = inv0;
+                motor4.pul = inv0;
+                
+                //Pulses set1 every 3 pulses of set0
+                if(j%3 == 0){
+                    inv1 =~ inv1;   //Inverts Value
+                    motor1.pul = inv1;
+                    motor3.pul = inv1;
+                    motor5.pul = inv1;
+                }
+                wait_us(pulseLength);
+             }
+             altSet = true;
+        }else if(altSet == true){
+             //Pulses for total desired Rotation for set1
+             for(int j=0; j<ROTATION; j++){
+                inv1 =~ inv1;   //Inverts Value
+                motor1.pul = inv1;
+                motor3.pul = inv1;
+                motor5.pul = inv1;
+                
+                //Pulses set0 every 3 pulses of set1
+                if(j%3 == 0){
+                    inv0 =~ inv0;   //Inverts Value
+                    motor0.pul = inv0;
+                    motor2.pul = inv0;
+                    motor4.pul = inv0;
+                }
+                wait_us(pulseLength);
+            }
+             altSet = false;
+        }
+    }
+}