Hoe maak je een All-richting voertuig met de wielen van de Mecanum (7 / 8 stap)

Stap 7: programmeren

De snelheid van vier Mecanum wielen worden bepaald door de hoeksnelheid

en snelheden in de x-as en y-as van het voertuig. Een Joystick wordt gebruikt voor de controle van de snelheid van het platform met de linker stick voor translatietraagheden velocity, en juiste stick voor hoeksnelheid. Sluit een USB-Host Me module op poort 3 van Orion bestuur en sluit een draadloze module op Me USB Host module.

Download Makeblock bibliotheek en zet het onder Arduino bibliotheek. De Arduino-codes zijn als volgt:

 #include "Wire.h" #include "SoftwareSerial.h" #include "MeOrion.h" MeUSBHost joypad(PORT_3); MeEncoderMotor motor1(0x02, SLOT2); MeEncoderMotor motor2(0x02, SLOT1); MeEncoderMotor motor3(0x0A, SLOT2); MeEncoderMotor motor4(0x0A, SLOT1); float linearSpeed = 100; float angularSpeed = 100; float maxLinearSpeed = 200; float maxAngularSpeed = 200; float minLinearSpeed = 30; float minAngularSpeed = 30; void setup() { motor1.begin(); motor2.begin(); motor3.begin(); motor4.begin(); Serial.begin(57600); joypad.init(USB1_0); } void loop() { Serial.println("loop:"); //setEachMotorSpeed(100, 50, 50, 100); if(!joypad.device_online) { Serial.println("Device offline."); joypad.probeDevice(); delay(1000); } else { int len = joypad.host_recv(); parseJoystick(joypad.RECV_BUFFER); delay(5); } //delay(500); } void setEachMotorSpeed(float speed1, float speed2, float speed3, float speed4) { motor1.runSpeed(speed1); motor2.runSpeed(-speed2); motor3.runSpeed(-speed3); motor4.runSpeed(-speed4); } void parseJoystick(unsigned char *buf) //Analytic function, print 8 bytes from USB Host { // debug joystick // int i = 0; // for(i = 0; i < 7; i++) // { // Serial.print(buf[i]); // Serial.print('-'); // } // Serial.println(buf[7]); // delay(10); // increase and decrease speed switch (buf[5]) { case 1: linearSpeed += 5; if (linearSpeed > maxLinearSpeed) { linearSpeed = maxLinearSpeed; } break; case 2: angularSpeed += 5; if (angularSpeed > maxAngularSpeed) { angularSpeed = maxAngularSpeed; } break; case 4: linearSpeed -= 5; if (linearSpeed < minLinearSpeed) { linearSpeed = minLinearSpeed; } break; case 8: angularSpeed -= 5; if (angularSpeed < minAngularSpeed) { angularSpeed = minAngularSpeed; } break; default: break; } if ((128 != buf[0]) || (127 != buf[1]) || (128 != buf[2]) || (127 != buf[3])) { float x = ((float)(buf[2]) - 127) / 128; float y = (127 - (float)(buf[3])) / 128; float a = (127 - (float)(buf[0])) / 128; mecanumRun(x * linearSpeed, y * linearSpeed, a * angularSpeed); } else { switch (buf[4]) { case 0: mecanumRun(0, linearSpeed, 0); break; case 4: mecanumRun(0, -linearSpeed, 0); break; case 6: mecanumRun(-linearSpeed, 0, 0); break; case 2: mecanumRun(linearSpeed, 0, 0); break; case 7: mecanumRun(-linearSpeed / 2, linearSpeed / 2, 0); break; case 5: mecanumRun(-linearSpeed / 2, -linearSpeed / 2, 0); break; case 1: mecanumRun(linearSpeed / 2, linearSpeed / 2, 0); break; case 3: mecanumRun(linearSpeed / 2, -linearSpeed / 2, 0); break; default: mecanumRun(0, 0, 0); break; } } } void mecanumRun(float xSpeed, float ySpeed, float aSpeed) { float speed1 = ySpeed - xSpeed + aSpeed; float speed2 = ySpeed + xSpeed - aSpeed; float speed3 = ySpeed - xSpeed - aSpeed; float speed4 = ySpeed + xSpeed + aSpeed; float max = speed1; if (max < speed2) max = speed2; if (max < speed3) max = speed3; if (max < speed4) max = speed4; if (max > maxLinearSpeed) { speed1 = speed1 / max * maxLinearSpeed; speed2 = speed2 / max * maxLinearSpeed; speed3 = speed3 / max * maxLinearSpeed; speed4 = speed4 / max * maxLinearSpeed; } setEachMotorSpeed(speed1, speed2, speed3, speed4); }