Diff: main.cpp

Revision:

0:67b964961693

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sun Mar 07 20:13:02 2021 +0000
@@ -0,0 +1,216 @@
+#include "mbed.h"
+#include <motor.h>
+#include "rtos.h"
+
+#define TX        p9
+#define RX        p10
+
+#define encoder_baud        9600
+#define encoder_resolution  (1 << 12)
+#define resolution 12
+
+float encoderPositionToAngle(uint16_t position);
+bool checksum(uint16_t data);
+
+Serial pc(USBTX, USBRX); // tx, rx
+Serial encoder(TX, RX, encoder_baud);
+
+Motor motor0(p5,p6,p7);
+Motor motor1(p15,p16,p17);
+Motor motor2(p18,p19,p20);  //pull,drive,enable
+Motor motor3(p21,p22,p23);
+Motor motor4(p24,p25,p26);
+Motor motor5(p27,p28,p29);
+
+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;
+    }
+}
+
+void motorFunc(){
+    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 = false;    //THIS IS WHAT I CHANGED
+        }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;
+        }
+    }   
+}
+
+void encoderFunc(){
+    pc.printf("Hello World!\n\r");
+    char D[1] = {0x00};
+    //char G[1] = {0xFF};
+    uint16_t position = 0;
+    pc.printf("Talking to Encoder\n\r");
+    while(1) {
+        encoder.putc(D[0]);
+        pc.printf("MSG");
+        if(encoder.readable()) {
+            position = encoder.getc();  //Lower Byte
+            position |= encoder.getc()<< 8;    //Upper Byte
+            
+            //if(!(checksum(position)==1)){
+                position &= 0x3FFF;             //Removing Check Bits
+                if (resolution == 12) position = position >> 2;
+                float angle = encoderPositionToAngle(position);
+                pc.printf("Position: %d\n\rAngle: %f\n\r",position, angle);
+                pc.printf("\n\r\n\r\n\r");
+                if(angle<180 && angle>160){
+                    //motor1.ena=LOW;
+                    pc.printf("STOP");
+                }else{
+                    //motor1.ena=HIGH;   
+                    pc.printf("GO");
+                }
+            //}/*else{
+                //pc.printf("FAILED CHECKSUM!");
+                //pc.printf("\n\r\n\r\n\r");
+          //  }*/
+            
+        }
+        wait(0.5);
+    }
+    
+    /*
+    pc.printf("Hello World!\n\r");
+    Encoder encoder(0xF0);
+    while(1){
+        pc.printf("Talking to Encoder\n\r");   
+        encoder.updatePosition();
+        pc.printf("Position: %d",encoder.getPosition(););
+        pc.printf("\n\r\n\r\n\r");
+    }
+    */
+}
+
+
+Thread encoderThread;
+Thread motorThread;
+
+int main() {
+    encoderThread.start(encoderFunc);
+    motorThread.start(motorFunc);
+    
+}
+//Assuming Odd Parity
+bool checksum(uint16_t data){
+    bool checksum = 0;
+    uint8_t cbe = 0;    //Even Checker
+    uint8_t cbo = 0;    //Odd Checker
+    
+    if(data & (1<<14)){ cbe++; }
+    if(data & (1<<15)){ cbo++; }
+    
+    for(int i=0; i<8; i++){
+        if(data & (1<<i*2)){ cbe++; }       //Checking Even Bits
+        if(data & (1<<(i*2+1))){ cbo++; }   //Checking Odd Bits
+    }
+    
+    if(!(cbe%2==1 && cbo%2==1)){ checksum = 1; }
+    return checksum;
+}
+
+float encoderPositionToAngle(uint16_t position){
+    return (float)position/11.3777;
+}
\ No newline at end of file