Matthew Gasper
Combination of Stepper Motors and Encoders
Revision 0:67b964961693, committed 2021-03-07
- Comitter:
- P3nguin18
- Date:
- Sun Mar 07 20:13:02 2021 +0000
- Commit message:
- Base;
Changed in this revision
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/Encoder.cpp Sun Mar 07 20:13:02 2021 +0000
@@ -0,0 +1,19 @@
+#include <encoder.h>
+/*
+Encoder::Encoder(char _address){address == _address;}
+void Encoder::updatePosition(Serial& SerialPort){
+ serialPort.putc(address); //Encoder Read Command
+ if(encoder.readable()) {
+ position = encoder.getc() << 8; //Lower Byte
+ position |= encoder.getc(); //Upper Byte
+ position &= 0x3FFF; //Removing Check Bits
+ if (resolution == 12) position = position >> 2;
+ }
+}
+uint16_t Encoder::getPosistion(){return position;}
+
+/*
+float encoderPositionToAngle (uint16_t position){
+ return (float)((position * 1.0) / ( 2 ^ resolution));
+}
+*/
\ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/encoder.h Sun Mar 07 20:13:02 2021 +0000
@@ -0,0 +1,12 @@
+#include <mbed.h>
+#define resolution = 12
+/*
+class Encoder{
+ private:
+ char address[1] = {0x00};
+ uint16_t position = 0;
+ public:
+ Encoder(char _address){}
+ void updatePosition(Serial* serialPort);
+ uint16_t getPosistion();
+} */
\ No newline at end of file
--- /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
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed-rtos.lib Sun Mar 07 20:13:02 2021 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/mbed_official/code/mbed-rtos/#5713cbbdb706
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Sun Mar 07 20:13:02 2021 +0000 @@ -0,0 +1,1 @@ +https://os.mbed.com/users/mbed_official/code/mbed/builds/65be27845400 \ No newline at end of file
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/motor.cpp Sun Mar 07 20:13:02 2021 +0000 @@ -0,0 +1,3 @@ +#include "mbed.h" +#include <motor.h> +
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/motor.h Sun Mar 07 20:13:02 2021 +0000
@@ -0,0 +1,11 @@
+#include <mbed.h>
+
+class Motor{
+ public:
+ Motor( PinName _pul, PinName _dir, PinName _ena) :
+ pul(_pul), dir(_dir), ena(_ena){}
+
+ DigitalOut pul;
+ DigitalOut dir;
+ DigitalOut ena;
+};
\ No newline at end of file