Estos códigos son para el robot robot car.
#include "SPI.h"
#include "Mirf.h"
#include "nRF24L01.h"
#include "MirfHardwareSpiDriver.h"
#include "Wire.h"
#include "I2Cdev.h"
#include "MPU6050.h"
#include "HMC5883L.h"
#include "Servo.h"
Servo Servo_8;
Servo Servo_9;
Servo Servo_10;
Servo Servo_11;
Servo Servo_12;
Servo Servo_13;
int IN1=23;
int IN2=22;
int IN3=24;
int IN4=25;
int ENA=5 ;
int ENB=4;
int LED1=11;
int LED2=12;
float k_8;
float k_9;
float k_10;
float k_11;
float k_12;
float k_13;
float speed_9=1500;
float speed_8=1500;
float speed_10=1500;
float speed_11=1500;
float speed_12=1500;
float speed_13=1500;
struct Axis // Datas from remote control
{
uint16_t axis_1;
uint16_t axis_2;
uint16_t axis_3;
uint16_t axis_4;
uint16_t axis_5;
uint16_t axis_6;
uint16_t axis_7;
uint16_t axis_8;
};
Axis axis_x;
struct Gesture // Datas send back to remote control
{
long Joystick_1_X;
long Joystick_1_Y;
long Joystick_2_X;
long Joystick_2_Y;
long Joystick_1;
long Joystick_2;
uint16_t null_1;
uint16_t null_2;
};
Gesture data;
void setup()
{
Wire.begin();
Serial.begin(115200);
///////////////////////////////////////////
pinMode(IN1,OUTPUT);
pinMode(IN2,OUTPUT);
pinMode(IN3,OUTPUT);
pinMode(IN4,OUTPUT);
pinMode(ENA,OUTPUT);
pinMode(ENB,OUTPUT);
pinMode(LED1,OUTPUT);
pinMode(LED2,OUTPUT);
digitalWrite(IN1,HIGH);
digitalWrite(IN2,HIGH);
digitalWrite(IN3,HIGH);
digitalWrite(IN4,HIGH);
Servo_8.attach(8);
Servo_9.attach(9);
Servo_10.attach(10);
Servo_11.attach(11);
Servo_12.attach(12);
Servo_13.attach(13);
/* Part of the protection codes.If he robot was turned on with the angle over 45(-45) degrees,the wheels
will not spin until the robot is in right position. */
// 24L01 initialization
Mirf.cePin = 53;
Mirf.csnPin = 48;
Mirf.spi = &MirfHardwareSpi;
Mirf.init();
Mirf.setRADDR((byte *)"serv1");
Mirf.payload = 1
//////////////////////////////////////////////
}
void loop()
{
Recive();
Arm_Control();
}
//////////////////////////////////////////////////////
void Recive()
{
if(!Mirf.isSending() && Mirf.dataReady()){ // Read datas from the romote contYawer
Mirf.getData((byte *) &axis_x);
//Serial.print("data="); Serial.print(axis_x);
//Serial.print("axis_1=");
//Serial.println(axis_x.axis_1);
/*Serial.print(" axis_2=");
Serial.print(axis_x.axis_2);
Serial.print(" axis_3=");
Serial.print(axis_x.axis_3);
Serial.print(" axis_4=");
Serial.print(axis_x.axis_4);
Serial.print(" axis_5=");
Serial.print(axis_x.axis_5);
Serial.print(" axis_6=");
Serial.print(axis_x.axis_6);
Serial.print(" axis_7=");
Serial.print(axis_x.axis_7);
Serial.print(" axis_8=");
Serial.println(axis_x.axis_8);*/
Mirf.setTADDR((byte *)"clie1");
Mirf.send((byte *) &data); // Send datas back to the contYawer
}
else
{
axis_x.axis_1 = axis_x.axis_2 = 500;
}
}
void Arm_Control()
{
if((axis_x.axis_5==1)&&(axis_x.axis_1 > 550))
{
k_10 = -map(axis_x.axis_1, 531, 1023, 0, 500)*0.01;
}
else if((axis_x.axis_5==1)&&(axis_x.axis_1 < 450))
{
k_10 = -map(axis_x.axis_1, 0, 469, -500, 0)*0.01;
}
else{
k_10 = 0;
}
if((axis_x.axis_5==1)&&(axis_x.axis_2 > 550))
{
k_8 = -map(axis_x.axis_2, 531, 1023, 0, 500)*0.01;
}
else if((axis_x.axis_5==1)&&(axis_x.axis_2 < 450))
{
k_8 = -map(axis_x.axis_2, 0, 469, -500, 0)*0.01;
}
else{
k_8 = 0;
}
if((axis_x.axis_5==0)&&(axis_x.axis_2 > 550))
{
k_9 = -map(axis_x.axis_2, 531, 1023, 0, 500)*0.01;
}
else if((axis_x.axis_5==0)&&(axis_x.axis_2 < 450))
{
k_9 = -map(axis_x.axis_2, 0, 469, -500, 0)*0.01;
}
else{
k_9 = 0;
}
if((axis_x.axis_5==0)&&(axis_x.axis_1 > 550))
{
k_12= -map(axis_x.axis_1, 531, 1023, 0, 500)*0.01;
}
else if((axis_x.axis_5==0)&&(axis_x.axis_1 < 450))
{
k_12= -map(axis_x.axis_1, 0, 469, -500, 0)*0.01;
}
else{
k_12 = 0;
}
if((axis_x.axis_6==0)&&(axis_x.axis_4 > 550))
{
k_13= -map(axis_x.axis_4, 531, 1023, 0, 500)*0.01;
}
else if((axis_x.axis_6==0)&&(axis_x.axis_4 < 450))
{
k_13= -map(axis_x.axis_4, 0, 469, -500, 0)*0.01;
}
else{
k_13 = 0;
}
if((axis_x.axis_6==1)&&(axis_x.axis_4 > 550))
{
k_11= -map(axis_x.axis_4, 531, 1023, 0, 500)*0.001;
}
else if((axis_x.axis_6==1)&&(axis_x.axis_4 < 450))
{
k_11= -map(axis_x.axis_4, 0, 469, -500, 0)*0.001;
}
else{
k_11 = 0;
}
speed_8 = min(2000, max(800, speed_8 += k_8));
speed_9 = min(1800, max(1000, speed_9 += k_9));
speed_10 = min(2500, max(500, speed_10 += k_10));
speed_11 = min(1800, max(1000, speed_11 += k_11));
speed_12 = min(1800, max(1000, speed_12 += k_12));
speed_13 = min(1900, max(1200, speed_13 += k_13));
Serial.print(" speed_8="); Serial.print(speed_8);Serial.print(" speed_9="); Serial.print(speed_9);
Serial.print(" speed_10="); Serial.print(speed_10);Serial.print(" speed_11="); Serial.print(speed_11);
Serial.print(" speed_12="); Serial.print(speed_12);Serial.print(" speed_13="); Serial.println(speed_13);
Servo_9.writeMicroseconds(speed_9);
Servo_8.writeMicroseconds(speed_8);
Servo_10.writeMicroseconds(speed_10);
Servo_11.writeMicroseconds(speed_11);
Servo_12.writeMicroseconds(speed_12);
Servo_13.writeMicroseconds(speed_13);
}