Matthew Gasper
/
Synergy
Combination of Stepper Motors and Encoders
main.cpp
- Committer:
- P3nguin18
- Date:
- 2021-03-07
- Revision:
- 0:67b964961693
File content as of revision 0:67b964961693:
#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; }