Changed Motor Controls to load from file #10

Author: Ximidar

scarborough/src/Arduino_Communications/arduino_i2c.cpp

@@ -5,12 +5,10 @@
  *      Author: sdcr
  */
 #include "arduino_i2c.h"
-#include "arduino_comm.h"
 
 I2Cdev i2cdev;
 
 ArdI2C i2c;
-ArdComm ardcomm;
 Handler handler;
 
 double imu_data[3];
@@ -45,18 +43,19 @@ int main(int argc, char **argv){
 	ros::NodeHandle n;
 
 	ros::Subscriber imu;
-	ros::Publisher ard_pub = n.advertise<scarborough::Motor_Speed>(handler.MOTORS, 100);
 	ros::Publisher depth_pub = n.advertise<scarborough::Depth>(handler.DEPTH_SENSOR, 100);
 	ros::Publisher kill_pub = n.advertise<scarborough::Kill_Switch>(handler.KILL, 10);
 
 	ros::Subscriber sub;
 
-	i2c.init(); // does nothing.
+	i2c.init(); 
 
 	imu = IMU.subscribe(handler.IMU, 200, getdata);
 	sub = n.subscribe(handler.DESIRED, 200, getdata_DESIRED);
 	int kill;
 	double depth;
+	scarborough::Kill_Switch kill_switch;
+	scarborough::Depth depth_sensor;
 
 
 	while(ros::ok()){
@@ -67,24 +66,24 @@ int main(int argc, char **argv){
 		kill = i2c.read_kill();
 		depth = i2c.read_depth();
 
-		ardcomm.depth.depth = depth;
+		depth_sensor.depth = depth;
 
 		if(kill == 1){
-			ardcomm.kill_switch.killed = false;
+			kill_switch.killed = false;
 		}
 		else{
-			ardcomm.kill_switch.killed = true;
+			kill_switch.killed = true;
 		}
 
 
 		//publish motor data to the ARD_I2C
 		//ard_pub.publish(ardcomm.motor);
-		depth_pub.publish(ardcomm.depth);
-		kill_pub.publish(ardcomm.kill_switch);
+		depth_pub.publish(depth_sensor);
+		kill_pub.publish(kill_switch);
 
 		//cout << ardcomm.motor << endl;
-		cout << ardcomm.depth << endl;
-		cout << ardcomm.kill_switch << endl;
+		cout << depth_sensor << endl;
+		cout << kill_switch << endl;
 
 
 
@@ -107,8 +106,7 @@ ArdI2C::ArdI2C(){
 //use this if ever needed... It used to do something but is now a ghost of algorithms past
 void ArdI2C::init(){
 
-	//write the pid values to the teensy
-	//pid_Set();
+	restart_arduino();
 
 
 }
@@ -197,9 +195,6 @@ float ArdI2C::read_depth(){
 
 	combined = d[0] | (d[1] << 8);
 
-
-	cout << combined << endl;
-
 	//we multiplied it by 1000 on the arduino side
 	float result = (float)(combined) / 1000.00;
 
@@ -242,6 +237,17 @@ void ArdI2C::update_desired(scarborough::Desired_Directions _desired){
 
 }
 
+void ArdI2C::restart_arduino(){
+	//write 1 to the arduino at the 15 address to restart it
+	i2cdev.writeByte(0x04, 15, 1);
+
+}
+void ArdI2C::restart_depth_sensor(){
+	//write 1 to the arduino at the 15 address to reinitialize the depth sensor
+	i2cdev.writeByte(0x04, 16, 1);
+
+}
+
 //reg 15 - 38
 void ArdI2C::pid_Set(){
 	cout<<"Initializing pid values" <<endl;

scarborough/src/Arduino_Communications/arduino_i2c.h

@@ -31,6 +31,8 @@
 #include "scarborough/YPR.h"
 #include "scarborough/Desired_Directions.h"
 #include "../HandlerNames/HANDLER_NAMES.h"
+#include "scarborough/Kill_Switch.h"
+#include "scarborough/Depth.h"
 #include <math.h>
 #include <stdint.h>
 #include <inttypes.h>
@@ -42,6 +44,8 @@ class ArdI2C{
 public:
 	ArdI2C();
 	void get_accel_info(); // This will query the correct ROS stream to get the IMU data
+	void restart_arduino();
+	void restart_depth_sensor();
 	string ardRead();
 	int read_kill();
 	float read_depth();

scarborough/src/Motor_Controls/Helm.cpp

@@ -7,12 +7,8 @@ Helm::Helm(){
 	ROLL_CONST = 1;
 	DEPTH_CONST = 1;
 
-    //Here is the actual PID value for the robot
-	double pid[3] = {
-		65,
-		0,
-		0
-	};
+    //this will initialize the PID value
+	load_yaml();
     //this gives the PID the P, I and D values then limits their output with a min and a max
 	yaw_pid.initPid(pid[0], pid[1], pid[2], 1200, 1800);
 	pitch_pid.initPid(pid[0], pid[1], pid[2], 1200, 1800);
@@ -23,7 +19,48 @@ Helm::Helm(){
 
 void Helm::init(){
 	//initialize the time
-	last_time = ros::Time::now();
+	last_time = ros::Time::now();   
+
+
+}
+
+void Helm::load_yaml(){
+    ifstream config("/home/ros/PID_config.txt");
+
+    double P = 0.00;
+    double I = 0.00;
+    double D = 0.00;
+
+
+    for( string line; getline( config, line ); )
+    {
+        cout << line << endl;
+        switch(line[0]){
+
+            case 'P':
+                P = atof( line.substr(1, line.length()).c_str() );
+                break;
+            case 'I':
+                I = atof( line.substr(1, line.length()).c_str() );
+                break;
+            case 'D':
+                D = atof( line.substr(1, line.length()).c_str() );
+                break;
+            default:
+                break;
+
+
+        }
+    }
+
+    cout << "P: " << P << endl;
+    cout << "I: " << I << endl;
+    cout << "D: " << D << endl;
+
+    pid[0] = P;
+    pid[1] = I;
+    pid[2] = D;
+
 }
 
 

scarborough/src/Motor_Controls/Helm.h

@@ -12,7 +12,9 @@
 #include <stdlib.h>
 #include <algorithm>
 #include <string>
-
+#include <iostream>
+#include <string>
+#include <fstream>
 
 using namespace std;
 
@@ -36,11 +38,13 @@ class Helm{
 	void update_depth(scarborough::Depth _depth);
 	void computron();
 	void reset();
+	void load_yaml();
 
 	//returns the smallest absolute value angle equivalent to the given angle
 	static double normalizeAngle(const double& angle); //degrees
 
 	double YAW_CONST, PITCH_CONST, ROLL_CONST, DEPTH_CONST;
+	double pid[3];
 
 
 

scarborough/src/Motor_Controls/I2Cdev.cpp

@@ -172,7 +172,7 @@ int8_t I2Cdev::readWord(uint8_t devAddr, uint8_t regAddr, uint16_t *data, uint16
  */
 int8_t I2Cdev::readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, uint16_t timeout) {
     int8_t count = 0;
-    int fd = open("/dev/i2c-0",O_RDWR);
+    int fd = open("/dev/i2c-1",O_RDWR);
 
     if (fd < 0) {
         fprintf(stderr, "Failed to open device: %s\n", strerror(errno));
@@ -344,7 +344,7 @@ bool I2Cdev::writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_
         return(FALSE);
     }
 
-    fd = open("/dev/i2c-0", O_RDWR);
+    fd = open("/dev/i2c-1", O_RDWR);
     if (fd < 0) {
         fprintf(stderr, "Failed to open device: %s\n", strerror(errno));
         return(FALSE);
@@ -391,7 +391,7 @@ bool I2Cdev::writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16
         return(FALSE);
     }
 
-    fd = open("/dev/i2c-0", O_RDWR);
+    fd = open("/dev/i2c-1", O_RDWR);
     if (fd < 0) {
         fprintf(stderr, "Failed to open device: %s\n", strerror(errno));
         return(FALSE);

scarborough/src/Motor_Controls/PID_config.yaml

@@ -0,0 +1,5 @@
+PID:
+    P:60
+    I:0
+    D:0
+

scarborough/src/Motor_Controls/motors_scarborough.cpp

@@ -85,7 +85,6 @@ int main(int argc, char **argv){
 
 		//if the kill switch is off do not send motor signals
 		if(!killed){
-			cout << "Get Data for PID" << endl;
 			if(restart){
 				 motors.kill_motors();
 				 helm.reset();