#define Roll_initial 512
#define Pitch_initial 512
#define Yaw_initial 512
#define Angle_MarginP 0.5
#define Angle_Margin 0.5
#define Kd 0.01 // D 제어
#define Ky 0.3 // Roll 제어에 따른 Pitch 제어
#define Ki 0.05 // i제어
#define Motor_Speed 1000
#define Speed_High 700
#define Speed_Low 300
#define Kp1 0.5
#define ID_Roll 8
#define ID_Pitch 4
#define ID_Yaw 2
int flag=1;
int sequence1 =1;
double Kp=2.5; // P제어
int sa=0;
double wr=0;
double wp=0;
char *str;
char *p;
int temp;
double fRoll;
double fPitch;
double Roll, Pitch, Yaw;
long Motor_R, Motor_P, Motor_Y;
double ip=0;
double ir=0;
double Time = 0;
double dt = 0.025;
double maxw=0;
void setup()
{
Serial.begin(115200); //Serial Monitor
Serial1.begin(1000000); // Serial for Motor
Serial2.begin(115200); // Serial for IMU

Motor_R = Roll_initial;
Motor_P = Pitch_initial;
Motor_Y = Yaw_initial;
sequence1 =1;
Motor(512,512, 512, 100);
delay(1000);
Motor(513,600,513, 100);
delay(10);
}void loop()
{
//시리얼 통신으로 IMU 센서값 받아들임
temp=Serial2.available();
if(temp>40)
{
char b[60]={0};
char c[60]={0};
for(int i=0; i<temp; i++)
{
b[i]=Serial2.read();
}
for(int i=0; i<temp; i++)
{
if(b[i]==42)
{
for(int j=i; j<temp; j++)
{
if(b[j]==13)
{
for(int k=0; k<j-i-1; k++)
{
c[k]=b[k+i+1];
}
break;
}
}
}
}
str = strtok_r(c,”,”,&p);
Roll = atof(str);
str = strtok_r(NULL,”,”,&p);
Pitch = atof(str);
Time += dt;

Serial.print(fRoll);
Serial.print(“,”);
Serial.print(fPitch);
Serial.print(“,”);
Serial.print(Time);
Serial.println();
//시간에 따른 IMU 센서값 출력
delay(10);
}else{
Serial.begin(115200); //Serial Monitor
Serial1.begin(1000000); // Serial for Motor
Serial2.begin(115200); // Serial for IMU
Serial.println(“Loop Start”);
}
//0값 노이즈 제거
if(Pitch!=0){
wp=(fPitch-Pitch)/0.03;
ip+=fPitch*0.03;
fPitch=Pitch;

}
if(Roll!=0){

wr=(fRoll-Roll)/0.03; //Rol 각속도 측정
Serial.print(“w is – “);
Serial.print(wr);
if(maxw<abs(wr)&&abs(wr)<100){ //초기 값 세팅에서 손가락 길이 알기위한 w가 측정 노이즈 보정위해 100이상은 무시
maxw=abs(wr);

}
Serial.print(“max w is – “);
Serial.print(maxw);
ip+=fRoll*0.03;

fRoll=Roll;
}
if(sequence1){ //전원 연결시 스캐닝과정 초기 Roll Pitch 값이 0이 되도록 단순제어
if (0<abs(maxw)&&abs(maxw)<25){//w값에 따른 Kp값 변경
Kp=3;
}else if(26<abs(maxw)&&abs(maxw)<50){
Kp=2,3;
}else if(51<abs(maxw)&&abs(maxw)){
Kp=1.7;
}
if (abs(fPitch) > Angle_MarginP)
{

if(Pitch > 0){
if(Motor_P<824){
Motor_P+=1;
}
}
else if(fPitch < 0)
{
if(Motor_P>200){

Motor_P-=1;

}
}
}

if (abs(fRoll) > Angle_Margin)
{

if(fRoll < 0){
if(Motor_R<700){

Motor_R+=3;

}
}
else if(fRoll > 0)
{
if(Motor_R>300){
Motor_R-=3;

}
}
}
Motor(Motor_R,Motor_Y, Motor_P, 100);

}else{

//스캐닝 후 PID 제어

if (abs(fPitch) > Angle_MarginP)
{
Motor_P+=wp*Kd+Ki*ip;
if(Pitch > 0){
if(Motor_P<824){
Motor_P+=abs(fPitch)*Kp1;
}
}
else if(fPitch < 0)
{
if(Motor_P>200){

Motor_P-=abs(fPitch)*Kp1;

}
}
Motor_R+=Ky*abs(fPitch);
}

if (abs(fRoll) > Angle_Margin)
{

if(fRoll < 0){
if(Motor_R<700){
Motor_R+=wr*Kd+ip*Ki;
Motor_R+=abs(fRoll)*Kp;

}
}
else if(fRoll > 0)
{
if(Motor_R>300){
Motor_R-=abs(fRoll)*Kp;
Motor_R+=wr*Kd+ip*Ki;
}
}
}
Motor(Motor_R,Motor_Y, Motor_P, Motor_Speed); //모터와 시리얼 통신
}
if((abs(fRoll)<Angle_MarginP)&&(abs(fPitch)<Angle_MarginP)){
sequence1=0;
Motor_Y-=1;

}
Serial.println (sequence1);
delay(15);
}
void Motor(unsigned int Pos_1, unsigned int Pos_2, unsigned int Pos_3, unsigned int Speed) //모터 시리얼 통신 함수
{ unsigned char Check_Sum;
int Num_Actuator = 3;
unsigned char ID_1 = 8;
unsigned char ID_2 = 2;
unsigned char ID_3 = 4;
unsigned char Length = 5*Num_Actuator+4;

Serial1.write(0xFF);
Serial1.write(0xFF);
Serial1.write(0xFE); //Broadcast ID
Serial1.write(Length); //Length
Serial1.write(0×83); //Instruction
Serial1.write(0x1E); //Address
Serial1.write(4); //Data Length

Serial1.write(ID_1);
Serial1.write(Pos_1);
Serial1.write(Pos_1>>8);
Serial1.write(Speed);
Serial1.write(Speed>>8);
Serial1.write(ID_2);
Serial1.write(Pos_2);
Serial1.write(Pos_2>>8);
Serial1.write(Speed);
Serial1.write(Speed>>8);
Serial1.write(ID_3);
Serial1.write(Pos_3);
Serial1.write(Pos_3>>8);
Serial1.write(Speed);
Serial1.write(Speed>>8);
Check_Sum = ~(0xFE + Length + 0×83 + 0x1E + 4 + (ID_1+ID_2+ID_3) + (Pos_1+Pos_2+Pos_3) + ((Pos_1>>8)+(Pos_2>>8)+(Pos_3>>8)) + 3*(Speed+(Speed>>8)));
Serial1.write(Check_Sum);
}

#include <Servo.h>
#include <Wire.h>
#include <HMC5883L.h>
#include <string.h>
#define MAX_LEN 30
char cmd_array[MAX_LEN];
Servo servo_arm;
HMC5883L compass;
int error = 0;
int rignt_motor_A = 10;
int rignt_motor_B = 11;
int rignt_motor_power = 6;
volatile int right_wheel_count = 0;
int left_motor_A = 7;
int left_motor_B = 8;
int left_motor_power = 5;
volatile int left_wheel_count = 0;
int servo_arm_port = 9;
void setup()
{
Serial.begin(9600);
Wire.begin();

compass = HMC5883L();
error = compass.SetScale(1.3); //Set compass scale
if(error != 0) { Serial.println(“Compass ERR 1″);}
error = compass.SetMeasurementMode(Measurement_Continuous); //Set cont. measure mode
if(error != 0) { Serial.println(“Compass ERR 2″);}

attachInterrupt(0, right_wheel_encoder, CHANGE); // Right Wheel Encoder
attachInterrupt(1, left_wheel_encoder, CHANGE); // Left Wheel Encoder

servo_arm.attach(servo_arm_port);
servo_arm.write(140); //180 (max down) 140 (midpoint) 0(original)

pinMode(rignt_motor_A,OUTPUT);
pinMode(rignt_motor_B,OUTPUT);
pinMode(rignt_motor_power,OUTPUT);

pinMode(left_motor_A,OUTPUT);
pinMode(left_motor_B,OUTPUT);
pinMode(left_motor_power,OUTPUT);
}
void set_servo_angle(int angle)
{
servo_arm.write(angle);
delay(15);
}
void set_motor_value(int right_dir, int left_dir, int right_speed, int left_speed, int delay_time)
{

if(right_dir == 1) //front
{
digitalWrite(rignt_motor_A,HIGH);
digitalWrite(rignt_motor_B,LOW);
}
if(right_dir == 0) //back
{
digitalWrite(rignt_motor_A,LOW);
digitalWrite(rignt_motor_B,HIGH);
}

if(left_dir == 1) //front
{
digitalWrite(left_motor_A,HIGH);
digitalWrite(left_motor_B,LOW);
}
if(left_dir == 0) //back
{
digitalWrite(left_motor_A,LOW);
digitalWrite(left_motor_B,HIGH);
}

analogWrite(rignt_motor_power,right_speed);
analogWrite(left_motor_power,left_speed);

delay(delay_time);

digitalWrite(rignt_motor_power,0);
digitalWrite(left_motor_power,0);
}
float read_compass()
{
MagnetometerScaled scaled = compass.ReadScaledAxis();
int MilliGauss_OnThe_XAxis = scaled.XAxis;
float heading = atan2(scaled.YAxis+136, scaled.XAxis+40);
//float declinationAngle = 0.0457;
//heading += declinationAngle;

if(heading < 0) {heading += 2*PI;}
if(heading > 2*PI) {heading -= 2*PI;}

float deg_heading = ((heading * 180)/PI)-9;
if(deg_heading < 0) {deg_heading += 360;}

return deg_heading;
}
void right_wheel_encoder() {right_wheel_count++;}
void left_wheel_encoder() {left_wheel_count++;}
void send_all_sensor_val()
{
float heading = read_compass();

Serial.print(“r,”);
Serial.print(heading);
Serial.print(“,”);
Serial.print(right_wheel_count);
Serial.print(“,”);
Serial.print(left_wheel_count);
Serial.print(“,”);

MagnetometerRaw raw = compass.ReadRawAxis();
Serial.print(raw.XAxis);
Serial.print(“,”);
Serial.print(raw.YAxis);
Serial.print(“,”);
Serial.print(raw.ZAxis);
Serial.print(“,”);

Serial.println(“”);
right_wheel_count = 0;
left_wheel_count = 0;
}
void loop()
{
if(Serial.available()>0)
{
String cmd = Serial.readStringUntil(‘\n’);
//s,servo_angle,\n <- servo cmd
//s,140,\n
//m,right_fb,left_fb,right_pw,left_pw,time,\n <- move cmd
//m,1,0,125,125,500,\n
//r,0\n <- request cmd
char *ptr;
cmd.toCharArray(cmd_array,MAX_LEN);

char * header = strtok(cmd_array, “,”);
if(strcmp(header,”s”) == 0) //data request
{
int servo_val = atoi(strtok(NULL, “,”));
set_servo_angle(servo_val);
}
if(strcmp(header,”m”) == 0) //move command
{

int right_direction = atoi(strtok(NULL, “,”));
int left_direction = atoi(strtok(NULL, “,”));
int right_power = atoi(strtok(NULL, “,”));
int left_power = atoi(strtok(NULL, “,”));
int power_time = atoi(strtok(NULL, “,”));

set_motor_value(right_direction, left_direction, right_power, left_power, power_time);
}

if(strcmp(header,”r”) == 0) //data request
{
send_all_sensor_val();
}
}
delay(200);
}

#!/usr/bin/python
import BaseHTTPServer
import SimpleHTTPServer
import SocketServer
import json
import serial
STOP_CODE = 0
HEADING = 0
L_ENCODER = 0
R_ENCODER = 0
#com = serial.Serial(“/dev/ttyACM0″)
com = serial.Serial(“COM11″)
#LOG_FILE = open(“rover_log.txt”,’w')
class ThreadServer(SocketServer.ThreadingMixIn, BaseHTTPServer.HTTPServer):
pass
#
class MyRequestHandler(SimpleHTTPServer.SimpleHTTPRequestHandler):
def do_GET(self):
return SimpleHTTPServer.SimpleHTTPRequestHandler.do_GET(self)
#
def do_POST(self):
global STOP_CODE
global HEADING
global L_ENCODER
global R_ENCODER
global LOG_FILE

length = int(self.headers.getheader(‘content-length’))
var = self.rfile.read(length) #code=f&data=125&delay=500, code=s&data=57
packet = var.split(“&”)
cmd = packet[0].split(“=”)[1]

if cmd == “f”: #Front
header = “m,”
right_dir = “1,”
left_dir = “1,”
right_pw = packet[1].split(“=”)[1] left_pw = packet[1].split(“=”)[1] time_delay = packet[2].split(“=”)[1] ser_cmd = header + right_dir + left_dir + right_pw + “,” + left_pw + “,” + time_delay + “,\n”
print ser_cmd
com.write(ser_cmd)
#”"”
com.write(‘r,0\n’)
raw_sensor_str = com.readline() #r,heading,rencoder,lencoder,mag_x,mag_y,mag_z,\n
#”"”
sensor_data = raw_sensor_str.split(“,”)
HEADING = sensor_data[1] L_ENCODER = sensor_data[3] R_ENCODER = sensor_data[2] #LOG_FILE.write(ser_cmd)
#LOG_FILE.write(raw_sensor_str)

if cmd == “b”: #Back
header = “m,”
right_dir = “0,”
left_dir = “0,”
right_pw = packet[1].split(“=”)[1] left_pw = packet[1].split(“=”)[1] time_delay = packet[2].split(“=”)[1] ser_cmd = header + right_dir + left_dir + right_pw + “,” + left_pw + “,” + time_delay + “,\n”
print ser_cmd
com.write(ser_cmd)
#”"”
com.write(‘r,0\n’)
raw_sensor_str = com.readline() #r,heading,rencoder,lencoder,mag_x,mag_y,mag_z,\n
#”"”
sensor_data = raw_sensor_str.split(“,”)
HEADING = sensor_data[1] L_ENCODER = sensor_data[3] R_ENCODER = sensor_data[2] #LOG_FILE.write(ser_cmd)
#LOG_FILE.write(raw_sensor_str)
if cmd == “r”: #Right
header = “m,”
right_dir = “0,”
left_dir = “1,”
right_pw = packet[1].split(“=”)[1] left_pw = packet[1].split(“=”)[1] time_delay = packet[2].split(“=”)[1] ser_cmd = header + right_dir + left_dir + right_pw + “,” + left_pw + “,” + time_delay + “,\n”
print ser_cmd
com.write(ser_cmd)
#”"”
com.write(‘r,0\n’)
raw_sensor_str = com.readline() #r,heading,rencoder,lencoder,mag_x,mag_y,mag_z,\n
#”"”
sensor_data = raw_sensor_str.split(“,”)
HEADING = sensor_data[1] L_ENCODER = sensor_data[3] R_ENCODER = sensor_data[2] #LOG_FILE.write(ser_cmd)
#LOG_FILE.write(raw_sensor_str)
if cmd == “l”: #Left
header = “m,”
right_dir = “1,”
left_dir = “0,”
right_pw = packet[1].split(“=”)[1] left_pw = packet[1].split(“=”)[1] time_delay = packet[2].split(“=”)[1] ser_cmd = header + right_dir + left_dir + right_pw + “,” + left_pw + “,” + time_delay + “,\n”
print ser_cmd
com.write(ser_cmd)
#”"”
com.write(‘r,0\n’)
raw_sensor_str = com.readline() #r,heading,rencoder,lencoder,mag_x,mag_y,mag_z,\n
#”"”
sensor_data = raw_sensor_str.split(“,”)
HEADING = sensor_data[1] L_ENCODER = sensor_data[3] R_ENCODER = sensor_data[2] #LOG_FILE.write(ser_cmd)
#LOG_FILE.write(raw_sensor_str)
if cmd == “s”: #Servo
servo_angle = packet[1].split(“=”)[1] header = “s,”
ser_cmd = header + servo_angle + “,\n”
print ser_cmd
com.write(ser_cmd)
sensor_data_dict = {‘heading’:HEADING,’l_encoder’:L_ENCODER,’r_encoder’:R_ENCODER}

self.send_response(200)
self.send_header(“Content-type”, “text/html”)
self.end_headers()
self.wfile.write(json.dumps(sensor_data_dict))
#
#
if __name__ == “__main__”:
Handler = MyRequestHandler
server = ThreadServer((’0.0.0.0′,8080), Handler)

while not STOP_CODE == 1:
server.handle_request()
#LOG_FILE.close()
server.server_close()