#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()