Matthew Gasper
/
Synergy
Combination of Stepper Motors and Encoders
Diff: main.cpp
- Revision:
- 0:67b964961693
diff -r 000000000000 -r 67b964961693 main.cpp --- /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