int QEM [16] = {0,-1,1,0,1,0,0,-1,-1,0,0,1,0,1,-1,0}; // Quadrature Encoder Matrix int data; unsigned long prevUpdateTime; int kp = 10; int ki = 40; #define INTEGRATION_WINDOW 20 long intCount1 = 0; int prevEncVal1 = 0; int prevControl1 = 0; int targetSpeed1 = 0; int errorArray1[INTEGRATION_WINDOW]; #define MOTOR1_ENC_A 4 #define MOTOR1_ENC_B 5 #define MOTOR1_PWM 6 #define MOTOR1_DIR 7 long intCount2 = 0; int prevEncVal2 = 0; int prevControl2 = 0; int targetSpeed2 = 0; int errorArray2[INTEGRATION_WINDOW]; #define MOTOR2_ENC_A 8 #define MOTOR2_ENC_B 9 #define MOTOR2_PWM 10 #define MOTOR2_DIR 11 long intCount3 = 0; int prevEncVal3 = 0; int prevControl3 = 0; int targetSpeed3 = 0; int errorArray3[INTEGRATION_WINDOW]; #define MOTOR3_ENC_A 24 #define MOTOR3_ENC_B 25 #define MOTOR3_PWM 12 #define MOTOR3_DIR 22 long intCount4 = 0; int prevEncVal4 = 0; int prevControl4 = 0; int targetSpeed4 = 0; int errorArray4[INTEGRATION_WINDOW]; #define MOTOR4_ENC_A 26 #define MOTOR4_ENC_B 27 #define MOTOR4_PWM 13 #define MOTOR4_DIR 23 void setup() { Serial.begin(115200); prevUpdateTime = 0; pinMode(MOTOR1_ENC_A, INPUT); pinMode(MOTOR1_ENC_B, INPUT); pinMode(MOTOR1_PWM, OUTPUT); pinMode(MOTOR1_DIR, OUTPUT); pinMode(MOTOR2_ENC_A, INPUT); pinMode(MOTOR2_ENC_B, INPUT); pinMode(MOTOR2_PWM, OUTPUT); pinMode(MOTOR2_DIR, OUTPUT); pinMode(MOTOR3_ENC_A, INPUT); pinMode(MOTOR3_ENC_B, INPUT); pinMode(MOTOR3_PWM, OUTPUT); pinMode(MOTOR3_DIR, OUTPUT); pinMode(MOTOR4_ENC_A, INPUT); pinMode(MOTOR4_ENC_B, INPUT); pinMode(MOTOR4_PWM, OUTPUT); pinMode(MOTOR4_DIR, OUTPUT); for(int i=0; i 1) { Serial.println("got bytes"); int A = Serial.read(); if(A == 'H') { Serial.print(A); int B = Serial.read(); Serial.print(B); int command = A*256+B; if(command == 18498) { Serial.println("got correct header"); Serialread(); int leftmode = data; Serialread(); int leftspeed = data; Serialread(); int rightmode = data; Serialread(); int rightspeed = data; Serial.println(""); if(leftmode == 0) { targetSpeed1 = -leftspeed; targetSpeed4 = -leftspeed; } else { targetSpeed1 = leftspeed; targetSpeed4 = leftspeed; } if(rightmode == 0) { targetSpeed2 = -rightspeed; targetSpeed3 = -rightspeed; } else { targetSpeed2 = rightspeed; targetSpeed3 = rightspeed; } } else { Serial.println("wrong header"); } } } if(updateOldness >= 50000) { int wheelSpeed1 = intCount1; int wheelSpeed2 = intCount2; int wheelSpeed3 = intCount3; int wheelSpeed4 = intCount4; //Serial.print(intCount1); //Serial.print(" "); //Serial.print(intCount2); //Serial.print(" "); //Serial.print(intCount3); //Serial.print(" "); //Serial.println(intCount4); intCount1 = 0; intCount2 = 0; intCount3 = 0; intCount4 = 0; prevUpdateTime = now; prevControl1 = doControl(targetSpeed1, wheelSpeed1, prevControl1, MOTOR1_PWM, MOTOR1_DIR, errorArray1); prevControl2 = doControl(targetSpeed2, wheelSpeed2, prevControl2, MOTOR2_PWM, MOTOR2_DIR, errorArray2); prevControl3 = doControl(targetSpeed3, wheelSpeed3, prevControl3, MOTOR3_PWM, MOTOR3_DIR, errorArray3); prevControl4 = doControl(targetSpeed4, wheelSpeed4, prevControl4, MOTOR4_PWM, MOTOR4_DIR, errorArray4); } } int doControl(int targetSpeed, int measSpeed, int prevControl, int pwmPin, int dirPin, int * errorArray) { int error = measSpeed - targetSpeed; int errorIntegral = 0; //shift all values for(int i=1; i 255) { newControl = 255; } else if( newControl < -255) { newControl = -255; } int dir = 1; int realControl = newControl; if(realControl < 0) { realControl = -realControl; dir = 0; } analogWrite(pwmPin, realControl); digitalWrite(dirPin, dir); //Serial.print(dir); //Serial.print(" "); //Serial.println(realControl); return newControl; } void intMotor1() { processInt(MOTOR1_ENC_A, MOTOR1_ENC_B, &intCount1, &prevEncVal1, false); } void intMotor2() { processInt(MOTOR2_ENC_A, MOTOR2_ENC_B, &intCount2, &prevEncVal2, true); } void intMotor3() { processInt(MOTOR3_ENC_A, MOTOR3_ENC_B, &intCount3, &prevEncVal3, false); } void intMotor4() { processInt(MOTOR4_ENC_A, MOTOR4_ENC_B, &intCount4, &prevEncVal4, false); } void processInt(int channelA, int channelB, long * intCount, int * prevEncVal, boolean invertCount) { int a = digitalRead(channelA); int b = digitalRead(channelB); int val = (a << 1) + b; if(invertCount == false) { *intCount += QEM[*prevEncVal * 4 + val]; } else { *intCount -= QEM[*prevEncVal * 4 + val]; } *prevEncVal = val; } void Serialread() {//---------------------------------------------------------- Read serial port until data has been received ----------------------------------- do { data=Serial.read(); } while (data<0); Serial.print(data); }