Enkel 18dof Hexapod, Arduino nano (eventuellt med pololu maestro) (12 / 13 steg)

Steg 12:18 Servo kod

Förklaring:
Dokumentationen till http://www.pololu.com/docs/0J40/5.e

a. BYTE är en parameter som anger basen (format) till oss http://www.arduino.cc/en/Serial/Print

b. mål är ett positivt heltal mindre än 8192 (det kan skrivas i binära notation med 14 eller färre siffror)
t.ex. 6000, eller i binär: 01011101110000

c. 0x7F är 01111111 i binär, som infinity nollor till vänster, så "&" ing (bitvis AND) det döljer ut alla siffror i målet (när målet skrivs i binärt) utom de senast 7 siffrorna (endast 1 och 1 == 1. alla andra kombinationer == 0)
01011101110000
& 00000001111111
= 00000001110000

d. höger Skift operatör, skiftar sista sju siffror (nummer 7 till 13) i målet bort i tomma rymden och så nu de "nya" senast sju siffrorna var ursprungligen bits #0-6 (se färgkodade pololu doc). Mask med 0x7F igen, bara för att vara säker.
01011101110000, >> 7 till:
00000000101110, sedan:
& 00000001111111
= 00000000101110

Den största skillnaden från standardkoden är att jag mappade värden så att jag kunde tanklöst port koden från arduino-"Servo.write ()" - stil till pololu-"settarget ()" - stil.

void settarget (unsigned char servo, unsigned int mål)
{
Target = karta (mål, 0, 180, 2400, 9500);

Min kod, upp på github, är också kopierade nedan:
======================
#include < SoftwareSerial.h >
#define txPin 2
#define rxPin 3

#define TIBIA 25
#define dröjsmål 150

#define CW 70
#define CCW 105

#define upp 92
#define ner 125

// ~~~~~~~~~~~~~~~~~~~~ //

#define A_COX 1
#define A_FEM 2
#define A_TIB 3

#define B_COX 4
#define B_FEM 5
#define B_TIB 6

#define C_COX 7
#define C_FEM 8
#define C_TIB 9

#define D_COX 10
#define D_FEM 11
#define D_TIB 12
#define E_COX 13
#define E_FEM 14
#define E_TIB 15

#define F_COX 16
#define F_FEM 17
#define F_TIB 18

int numtimes = 3;

SoftwareSerial mySerial (rxPin, txPin);

void setup() / / kör en gång, när skissen startar
{
mySerial.begin(9600);
Delay(1000);
}
void loop()
{
för (int jag = 0; jag < = 4; i ++) {
walkfwd();
}
för (int j = 0; j < = 4; j ++) {
walkbwd();
}
för (int k = 0; k < = 2; k ++) {
turnleft();
}
för (int l = 0; l < = 2; l ++) {
turnright();
}
}

void tibia() {
settarget (A_TIB, TIBIA);
settarget (B_TIB, TIBIA);
settarget (C_TIB, TIBIA);
settarget (D_TIB, TIBIA);
settarget (E_TIB, TIBIA);
settarget (F_TIB, TIBIA);
}

// ~~~~~~~~~~fwd~~~~~~~~~~ //

void f1() {
[COXA] ändrat
settarget (A_COX, CW);
settarget (C_COX, CW);
settarget (E_COX, CCW);

settarget (D_COX, CW);
settarget (F_COX, CW);
settarget (B_COX, CCW);

Delay(delay);
}

void f2() {
[LÅRBENET] ändrat
settarget (A_FEM, ned);
settarget (C_FEM, ned);
settarget (E_FEM, ned);

settarget (D_FEM, UP);
settarget (F_FEM, UP);
settarget (B_FEM, UP);

Delay(delay);
}

void f3() {
[COXA] ändrat
settarget (A_COX, CCW);
settarget (C_COX, CCW);
settarget (E_COX, CW);

settarget (D_COX, CCW);
settarget (F_COX, CCW);
settarget (B_COX, CW);

Delay(delay);
}

void f4() {
[LÅRBENET] ändrat
settarget (A_FEM, UP);
settarget (C_FEM, UP);
settarget (E_FEM, UP);

settarget (D_FEM, ned);
settarget (F_FEM, ned);
settarget (B_FEM, ned);

Delay(delay);
}

// ~~~~~~~~~bwd~~~~~~~~~~~ //

void b1() {
[COXA] ändrat
settarget (A_COX, CCW);
settarget (C_COX, CCW);
settarget (E_COX, CW);

settarget (D_COX, CCW);
settarget (F_COX, CCW);
settarget (B_COX, CW);

Delay(delay);
}

void b2() {
[LÅRBENET] ändrat
settarget (A_FEM, ned);
settarget (C_FEM, ned);
settarget (E_FEM, ned);

settarget (D_FEM, UP);
settarget (F_FEM, UP);
settarget (B_FEM, UP);

Delay(delay);
}

void b3() {
[COXA] ändrat
settarget (A_COX, CW);
settarget (C_COX, CW);
settarget (E_COX, CCW);

settarget (D_COX, CW);
settarget (F_COX, CW);
settarget (B_COX, CCW);

Delay(delay);
}

void b4() {
[LÅRBENET] ändrat
settarget (A_FEM, UP);
settarget (C_FEM, UP);
settarget (E_FEM, UP);

settarget (D_FEM, ned);
settarget (F_FEM, ned);
settarget (B_FEM, ned);

Delay(delay);
}

// ~~~~~~~~~left~~~~~~~~~~~ //

void l1() {
[COXA] ändrat
settarget (A_COX, CCW);
settarget (C_COX, CCW);
settarget (E_COX, CCW);

settarget (D_COX, CW);
settarget (F_COX, CW);
settarget (B_COX, CW);

Delay(delay);
}

void l2() {
[LÅRBENET] ändrat
settarget (A_FEM, ned);
settarget (C_FEM, ned);
settarget (E_FEM, ned);

settarget (D_FEM, UP);
settarget (F_FEM, UP);
settarget (B_FEM, UP);

Delay(delay);
}

void l3() {
[COXA] ändrat
settarget (A_COX, CW);
settarget (C_COX, CW);
settarget (E_COX, CW);

settarget (D_COX, CCW);
settarget (F_COX, CCW);
settarget (B_COX, CCW);

Delay(delay);
}

void l4() {
[LÅRBENET] ändrat
settarget (A_FEM, UP);
settarget (C_FEM, UP);
settarget (E_FEM, UP);

settarget (D_FEM, ned);
settarget (F_FEM, ned);
settarget (B_FEM, ned);

Delay(delay);
}

// ~~~~~~~~~right~~~~~~~~~~~ //

void r1() {
[COXA] ändrat
settarget (A_COX, CW);
settarget (C_COX, CW);
settarget (E_COX, CW);

settarget (D_COX, CCW);
settarget (F_COX, CCW);
settarget (B_COX, CCW);

Delay(delay);
}

void r2() {
[LÅRBENET] ändrat
settarget (A_FEM, ned);
settarget (C_FEM, ned);
settarget (E_FEM, ned);

settarget (D_FEM, UP);
settarget (F_FEM, UP);
settarget (B_FEM, UP);

Delay(delay);
}

void r3() {
[COXA] ändrat
settarget (A_COX, CCW);
settarget (C_COX, CCW);
settarget (E_COX, CCW);

settarget (D_COX, CW);
settarget (F_COX, CW);
settarget (B_COX, CW);

Delay(delay);
}

void r4() {
[LÅRBENET] ändrat
settarget (A_FEM, UP);
settarget (C_FEM, UP);
settarget (E_FEM, UP);

settarget (D_FEM, ned);
settarget (F_FEM, ned);
settarget (B_FEM, ned);

Delay(delay);
}

// ~~~~~~~~~~~~~~~~~~~~ //

void walkbwd() {
Tibia();
B1();
B2();
B3();
B4();
}
void walkfwd() {
Tibia();
F1();
F2();
F3();
F4();
}

void turnleft() {
Tibia();
L1();
L2();
L3();
L4();
}

void turnright() {
Tibia();
R1();
R2();
R3();
R4();
}

// ~~~~~~~~~~~~~~~~~~~~ //

Skicka ett ange målet kommando till Maestro.
Målet är i enheter av kvartalet mikrosekunder, så normala intervall är 4000 till 8000.
void settarget (unsigned char servo, unsigned int mål)
{
Target = karta (mål, 0, 180, 2400, 9500);
mySerial.write(0xAA); Starta byte
mySerial.write(0x0C); enhets-id
mySerial.write(0x04); kommandonummer
mySerial.write(servo); servo nummer
mySerial.write (target & 0x7F);
mySerial.write ((mål >> 7) & 0x7F);
}