code for position logging for PID tuning

hwdata.c

@@ -136,6 +136,33 @@ int update_robot_pos(int32_t ang, int32_t x, int32_t y)
 	cur_pos_invalid_for_3dtof = 0;
 	pthread_mutex_unlock(&cur_pos_mutex);
 
+
+#ifdef MOTCON_PID_EXPERIMENT
+
+	static FILE* coord_log;
+	static int coord_log_inited = 0;
+
+	if(!coord_log_inited)
+	{
+		coord_log = fopen("coord_log.csv", "w");
+		if(!coord_log)
+		{
+			printf("ERROR: Opening coord_log.csv for write failed\n");
+		}
+		else
+		{
+			coord_log_inited = 1;
+			fprintf(coord_log, "ang_deg,x_mm,y_mm\n");
+		}
+	}
+
+	if(coord_log_inited)
+	{
+		fprintf(coord_log, "%.2f, %d, %d\n", ANG32TOFDEG(ang), x, y);
+	}
+
+#endif
+
 	return 0;
 }
 

makefile

@@ -12,6 +12,7 @@ all: rn1host
 CFLAGS += -DPULUTOF1
 CFLAGS += -DPULUTOF_ROBOT_SER_1_TO_4
 #CFLAGS += -DPULUTOF_ROBOT_SER_5_UP
+#CFLAGS += -DMOTCON_PID_EXPERIMENT
 
 %.o: %.c $(DEPS)
 	gcc -c -o $@ $< $(CFLAGS) -pthread

rn1host.c

@@ -1,5 +1,3 @@
-//#define MOTCON_PID_EXPERIMENT
-
 /*
 	PULUROBOT RN1-HOST Computer-on-RobotBoard main software