Boom och gripdon bot (14 / 15 steg)

Steg 14: Kodlista för robot-drivrutinen

/*
Bluetooth Boom-gripdon bot.

Mål i livet...
Följer dina kommandon skickas magiskt genom luften! Eller från USB :)

Skriven av Scott Beasley - 2015
Gratis att använda eller ändra. Njuta av.
*/

#include

Olika tidsfördröjningar används för körning och servo
#define TURNDELAY 475
#define TURNDELAY45 235

Pololu servo controller-befäl
#define SETSPEED 0x01
#define SETPOS 0x04

/*
Globals område.
*/

Skapa de motor, servo objekt att samverka med
SoftwareSerial Serial_servo (10, 11);
SoftwareSerial Serial_motors (10, 12);

Ogiltiga inställningar)
{
Ändra överföringshastigheter här om annorlunda än under
Serial.BEGIN (9600); Bluethooth anslutning
Serial_servo.BEGIN (9600);
Serial_motors.BEGIN (19200);

stoppa ();
set_servo_speed (0, 10); Servo rörelse hastighet
set_servo_speed (1, 10); att få dem att flytta lite mjukare
home_boom ();
home_gripper ();

fördröjning (500).
}

Ogiltig loop)
{
byte kommando = 0, val = 0;
Följande behöver bo intakt under livet av den
session så de görs "statisk". Detta håller oss från
att ha dem som globals
statiska byte dist = 1; Börja med korta drag
statiska byte hastighet = 1; långsam hastighet
statiska byte speed_offset = 40;
statiska byte turn_style = 1; och med vänder hårt
statiska byte boom_val = 60.
statiska byte gripper_val = 60.

om (Serial.available () > 0) {
Läs den inkommande kommandobyte
kommandot = Serial.read ();
}

Växla (kommandot) {
fallet "w":
go_forward (speed_offset);
Serial.println ("kommer fram");

om (dist == 1) {
fördröjning (750);
stoppa ();
} else om (dist == 2) {
fördröjning (1250);
stoppa ();
}

bryta;

fallet "z":
go_backward (speed_offset);
Serial.println ("går baklänges");

om (dist == 1) {
fördröjning (750);
stoppa ();
} else om (dist == 2) {
fördröjning (1250);
stoppa ();
}

bryta;

fallet "a":
go_left (speed_offset, turn_style);
fördröjning (TURNDELAY);
stoppa ();
Serial.println ("vrida vänster");
bryta;

fallet ":
go_right (speed_offset, turn_style);
fördröjning (TURNDELAY);
stoppa ();
Serial.println ("vrida rätt");
bryta;

fallet "q":
go_left (speed_offset, turn_style);
fördröjning (TURNDELAY45);
stoppa ();
Serial.println ("vrida vänster");
bryta;

fallet "e":
go_right (speed_offset, turn_style);
fördröjning (TURNDELAY45);
stoppa ();
Serial.println ("vrida rätt");
bryta;

fallet "h":
stoppa ();
Serial.println ("stoppa");
bryta;

fallet ">":
Gripdon X flytta skickar servo sätta värde
Val = Serial.read ();
gripper_val = val;

Vi begränsar värdet till verkliga movemovent gränserna för installationen
set_gripper (val);
Serial.println ("GripperX");
bryta;

fallet ' ^':
Gripdon Y flytta skickar servo sätta värde
Val = Serial.read ();
boom_val = val;

Vi begränsar värdet till verkliga movemovent gränserna för installationen
set_boom (val);
Serial.println ("GripperY");
bryta;

fallet "c":
gripper_val = 60.
boom_val = 60.
home_boom ();
home_gripper ();

Serial.println ("GripperHome");
bryta;

fallet "v":
Ange den spped bot kommer att flytta; 1 = långsammast, 3 = snabbast
hastighet = Serial.read ();

om (hastighet == 1) {
speed_offset = 40; Långsam
} else om (hastighet == 2) {
speed_offset = 20; Med
} else om (hastighet == 3) {
speed_offset = 0; Snabb!
}

Serial.println ("inställda hastigheten");
bryta;

fall skulle ":
Ange hur länge bot kommer att flytta; 1 = kort, 2 medium,
3 = kontinuerlig
dist = Serial.read ();
Serial.println ("flytta avstånd uppsättning");
bryta;

't i mål ":
Ange hur bot gör svängar, ett hjul (mjuka) eller två (svårt)
turn_style = Serial.read ();

Serial.println ("tur stil");
bryta;

fallet "+": / / Speed set från telefon
Speed ++;
om (hastighet > = 4)
hastighet = 1;

om (hastighet == 1) {
speed_offset = 40; Långsam
} else om (hastighet == 2) {
speed_offset = 20; Med
} else om (hastighet == 3) {
speed_offset = 0; Snabb!
}

bryta;

fallet '-': / / Dist flyttade från telefon
Dist ++;

om (dist > = 4)
dist = 1;
Serial.println ("flytta avstånd uppsättning");
bryta;

fallet ",": / / öppen gripare från telefon
gripper_val += 10.

om (gripper_val > = 179)
gripper_val = 179;

set_gripper (gripper_val);

Serial.println ("Gripper uppsättning");
bryta;

fallet '.': / / Stäng gripare från telefon
gripper_val-= 10.

om (gripper_val < = 0)
gripper_val = 0;

set_gripper (gripper_val);

Serial.println ("Gripper uppsättning");
bryta;

ärende '0': / / Sänk bommen från telefon
boom_val += 10.

om (boom_val > = 179)
boom_val = 179;

set_boom (boom_val);

Serial.println ("Boom uppsättning");
bryta;

ärende '1': / / höja bommen från telefonen
boom_val-= 10.

om (boom_val < = 0)
boom_val = 0;

set_boom (boom_val);

Serial.println ("Boom uppsättning");
bryta;

fall 255: / / skickas efter alla gripdon kommandon
Serial.flush ();
bryta;
}

Serial.flush ();
Delay(125);
}

void go_forward (byte speed_offset)
{
Serial.println ("framöver...");
Serial_motors.write ((byte) 1 + speed_offset);
Serial_motors.write ((byte) 128 + speed_offset);
}

void go_backward (byte speed_offset)
{
Serial.println ("går bakåt...");
Serial_motors.write ((byte) 127 - speed_offset);
Serial_motors.write ((byte) 255 - speed_offset);
}

void go_left (byte speed_offset, byte turn_style)
{
Serial.println ("kommer vänster...");
om (turn_style == 1)
Serial_motors.write ((byte) 1 + speed_offset);
annat
Serial_motors.write ((byte) 64); Stänga av vänstra motorn (mjuk tur)

Serial_motors.write ((byte) 255 - speed_offset);
}

void go_right (byte speed_offset, byte turn_style)
{
Serial.println ("kommer rätt...");

Serial_motors.write ((byte) 127 - speed_offset);

om (turn_style == 1)
Serial_motors.write ((byte) 128 + speed_offset);
annat
Serial_motors.write ((byte) 192); Stäng av den rätt motorn (mjuk tur)
}

void stopp)
{
Serial.println ("stopp!");
Serial_motors.write ((byte) 0);
}

Ange en servon position
servo_id är antalet servo (0-7)
vinkel är den abs positionen från 500 till 5500
void move_servo (int servo_id, int vinkel)
{
unsigned char buff [6] = {0x80, 0x01, SETPOS, 0x00, 0x00, 0x00};

unsigned int härda;
unsigned char data_hi, data_low;

Temp = vinkel & 0x1f80;
data_hi = temp >> 7.
data_low = vinkel & 0x7f;

buff [3] = servo_id; servo nummer
buff [4] = data_hi; data1
buff [5] = data_low; data2

Serial_servo.write (buff, 6);
Serial_servo.flush ();
}

void set_servo_speed (int servo_id, int hastighet)
{
unsigned char buff [5] = {0x80, 0x01, SETSPEED, 0x00, 0x00};
om (hastighet > 127)
hastighet = 127;

buff [3] = lowByte (servo_id);
buff [4] = lowByte (hastighet);
Serial_servo.write (buff, 5);
Serial_servo.flush ();
}

void set_boom (byte vinkel)
{
move_servo (0, karta (vinklar, 0, 179, 2500, 4000));
}

void set_gripper (byte vinkel)
{
move_servo (1, karta (vinklar, 0, 179, 2500, 4000));
}

void home_boom)
{
move_servo (0, 3000);
}

void home_gripper)
{
move_servo (1, 3000);
}