 int STOP_TIME = 200; //ms
 
 /* Photodiod pins */
-int diod1Pin = A4;
+int diod1Pin = A5;
 int diod2Pin = A5;
-int diod3Pin = A6;	
-int diod4Pin = A7;
+int diod3Pin = A5;  
+int diod4Pin = A5;
 
 bool collecting = true;
 int collectedPoints = 0;
-int numberOfCollections = 500;
+int numberOfCollections = 5;
 int numDataPoints = numberOfCollections * 4;
 double** dataPoints;
 
 enum Direction {FORWARD, BACKWARD};
 
-int aprintf(char *str, ...) {
-	int i, j, count = 0;
- 
-	va_list argv;
-	va_start(argv, str);
-	for(i = 0, j = 0; str[i] != '\0'; i++) {
-		if (str[i] == '%') {
-			count++;
- 
-			Serial.write(reinterpret_cast<const uint8_t*>(str+j), i-j);
- 
-			switch (str[++i]) {
-				case 'd': Serial.print(va_arg(argv, int));
-					break;
-				case 'l': Serial.print(va_arg(argv, long));
-					break;
-				case 'f': Serial.print(va_arg(argv, double));
-					break;
-				case 'c': Serial.print((char) va_arg(argv, int));
-					break;
-				case 's': Serial.print(va_arg(argv, char *));
-					break;
-				case '%': Serial.print("%");
-					break;
-				default:;
-			};
- 
-			j = i+1;
-		}
-	};
-	va_end(argv);
- 
-	if(i > j) {
-		Serial.write(reinterpret_cast<const uint8_t*>(str+j), i-j);
-	}
- 
-	return count;
-}
+void setup() {
+  Serial.begin(9600);
 
-void setup() {
-	Serial.begin(9600);
+  pinMode(enablePin_1, OUTPUT);
+  pinMode(in1Pin_1, OUTPUT);
+  pinMode(in2Pin_1, OUTPUT);
+  pinMode(enablePin_2, OUTPUT);
+  pinMode(in3Pin_2, OUTPUT);
+  pinMode(in4Pin_2, OUTPUT);
 
-	pinMode(enablePin_1, OUTPUT);
-	pinMode(in1Pin_1, OUTPUT);
-	pinMode(in2Pin_1, OUTPUT);
-	pinMode(enablePin_2, OUTPUT);
-	pinMode(in3Pin_2, OUTPUT);
-	pinMode(in4Pin_2, OUTPUT);
+  digitalWrite(diod1Pin, HIGH);
+  digitalWrite(diod2Pin, HIGH);
+  digitalWrite(diod3Pin, HIGH);
+  digitalWrite(diod4Pin, HIGH);
 
-	digitalWrite(diod1Pin, HIGH);
-	digitalWrite(diod2Pin, HIGH);
-	digitalWrite(diod3Pin, HIGH);
-	digitalWrite(diod4Pin, HIGH);
-
-	dataPoints = (double**)calloc(numDataPoints, sizeof(double*));
-
-	Serial.println("Car initialized, ready to go!");
+  // dataPoints = (double**)calloc(numDataPoints, sizeof(double*));
+  // for (int i = 0; i < numDataPoints; i++)
+  // {
+  //   dataPoints[i] = (double*) calloc(1, sizeof(double));
+  // }
+  dataPoints = (double **)malloc(numDataPoints * sizeof(double *));
+  for (int i = 0; i < numDataPoints; i++) {
+    dataPoints[i] = (double *)malloc(1 * sizeof(double));    
+  }
+  Serial.println("Car initialized, ready to go!");
 }
 
 void loop() {
-	Serial.println("loop");
-	if (collecting && collectedPoints <= numberOfCollections) {
-		/*driveRandom();*/
-		collectIntialData();
-		collectedPoints++;
-		delay(100);
-	} else if (collecting) {
-		computeMeans();
-		collecting = false;
-	} else {
-		mainLoop();
-	}
-
+  if (collecting && collectedPoints <= numberOfCollections) {
+    /*driveRandom();*/
+    collectIntialData();
+    collectedPoints++;
+  } else if (collecting) {
+    computeMeans();
+    collecting = false;
+  } else {
+    mainLoop();
+  }
+  delay(300);
 }
 
 void mainLoop() {
-	// RotateLeft(1000);
-	// RotateRight(1000);
-	// RotateLeft(1000);
-	// RotateRight(1000);
-	// Stop();
-	// delay(2000);
-	delay(2000);
+  // RotateLeft(1000);
+  // RotateRight(1000);
+  // RotateLeft(1000);
+  // RotateRight(1000);
+  // Stop();
+  // delay(2000);
+  delay(2000);
 }
 
 void Forward(int time)
 {
   Serial.println("move forward");
-	setMotors(DEFAULT_SPEED, FORWARD);
-	delay(time);
-	Stop();
-	delay(STOP_TIME);
+  setMotors(DEFAULT_SPEED, FORWARD);
+  delay(time);
+  Stop();
+  delay(STOP_TIME);
   /*digitalWrite(motor1PinA, HIGH);
   digitalWrite(motor1PinB, LOW);
   digitalWrite(motor2PinA, HIGH);

void RotateLeft(int time) // inverse voltage of only one motor
{
  Serial.println("rotate left");
	setMotorLeft(DEFAULT_SPEED, BACKWARD);
	setMotorRight(DEFAULT_SPEED, FORWARD);
	delay(time);
	Stop();
	delay(STOP_TIME);
  /*digitalWrite(motor1PinA, LOW);
  digitalWrite(motor1PinB, HIGH);
  digitalWrite(motor2PinA, HIGH);

void Backward(int time) // inverse voltage of both motors
{
  Serial.println("move backward");
  setMotors(DEFAULT_SPEED, BACKWARD);
  delay(time);
  Stop();
  delay(STOP_TIME);
  /*digitalWrite(motor1PinA, LOW);
  digitalWrite(motor1PinB, HIGH);
  digitalWrite(motor2PinA, LOW);
