Encoder working

Encoder.cpp

@@ -50,13 +50,15 @@ void Encoder::startThread(std::function<void(long enc, bool dir)> func, unsigned
 	if(th!=nullptr) delete th;
 
 	th = new std::thread([=](bool* running){
-		uint32_t enc;
+		uint32_t enc, oldEnc = 0;
 		bool dir;
 		while(*running) {
 			read(enc);
 			direction(dir);
-			if(enc>=threshold)
+			if(enc-oldEnc>=threshold) {
 				func((long)enc, dir);
+				oldEnc = enc;
+			}
 		}
 	}, std::ref(run));
 }
@@ -67,5 +69,7 @@ void Encoder::stopThread() {
 
 Encoder::~Encoder() {
 	// disable encoder
+	if(*run)
+		stopThread();
 	status = NiFpga_WriteU8(myrio_session, ENCACNFG, 0);
 }

Log.cpp

@@ -0,0 +1,20 @@
+#include "Log.h"
+
+using namespace std;
+
+Log::Log(std::string filename, char sep) : sep(sep) {
+	file.open("/home/"+filename);
+}
+
+void Log::println(long time, int dat1, long dat2) {
+	file << time << sep << dat1 << sep << dat2 << std::endl;
+}
+
+void Log::close() {
+	file.close();
+}
+
+Log::~Log() {
+	if(file.is_open())
+		file.close();
+}

Log.h

@@ -0,0 +1,18 @@
+#ifndef LOG_H
+#define LOG_H
+
+#include <iostream>
+#include <fstream>
+
+class Log {
+public:
+	Log(std::string filename, char sep = ' ');
+	~Log();
+	void println(long time, int dat1, long dat2);
+	void close();
+private:
+	std::ofstream file;
+	char sep;
+};
+
+#endif

Motor.cpp

@@ -3,10 +3,10 @@
 using namespace myRIO;
 
 // work only for PWM0 & PWM1
-Motor::Motor(uint32_t port) : speed(0), direction(CW),
+Motor::Motor(uint32_t port) : speed(0), direction(CCW),
 		pin(port==PWM0?A2:A3)
 {
-	channel = new PWM(port, 10e3, speed);
+	channel = new PWM(port, 1e3, speed);
 	MyRio_ReturnIfNotSuccess(status,
 			"PWM initialisation error");
 	enc = new Encoder(port!=PWM0); // if port==PWM0 : (port!=PWM0)=0=ENCA
@@ -34,5 +34,6 @@ void Motor::setInterrupt(std::function<void(long, bool)> func, unsigned int thre
 
 Motor::~Motor() {
 	channel->setDutyCycle(0);
+	enc->stopThread();
 }
 

MyRIO.h

@@ -15,6 +15,7 @@
 #include "Wifi.h"
 #include "Motor.h"
 #include "PID.h"
+#include "Log.h"
 
 
 

Time.cpp

@@ -27,10 +27,9 @@ long Time::elapsed_us() {
 
 	if(finalTime<startTime) {
 		long inRange = 1e6 - startTime;
-		finalTime += inRange;
-	}
-
-	return finalTime - startTime;
+		return finalTime + inRange;
+	} else
+		return finalTime - startTime;
 }
 
 long Time::elapsed_ms() {

Wifi.cpp

@@ -90,11 +90,11 @@ void Wifi::writeChar(char c) {
 }
 
 void Wifi::writeShort(short s) {
-	write(_socket, reinterpret_cast<const char*>(htons((uint16_t)&s)), sizeof(short));
+	write(_socket, reinterpret_cast<const char*>(&s), sizeof(short));
 }
 
 void Wifi::writeLong(long l) {
-	write(_socket, reinterpret_cast<const char*>(htonl((uint32_t)&l)), sizeof(long));
+	write(_socket, reinterpret_cast<const char*>(&l), sizeof(long));
 }
 
 Wifi::~Wifi() {

main.cpp

@@ -36,16 +36,30 @@ int main() {
 #ifdef DEF_MOTOR
 	Time t = Time::stopwatch();
 
+	Motor motorRight(PWM0);
+	motorRight.setSpeed(0);
+
 	Motor motorLeft(PWM1);
+	motorLeft.setDirection(CW);
+	int setpoint = 25;
 
+	Log log("log");
 	motorLeft.setInterrupt([&](long enc, bool dir) {
-		cout << t.elapsed_us() << "  " << enc << endl;
+		//cout << t.elapsed_us() << "  " << enc << endl;
+		log.println(t.elapsed_us(), setpoint, enc);
 		t.reset();
 	}, 1);
-
-	motorLeft.setSpeed(25);
-
-	Time::wait_s(10);
+	motorLeft.setSpeed(0);
+	Time::wait_s(1);
+	t.reset();
+	motorLeft.setSpeed(setpoint);
+	Time::wait_s(1);
+	setpoint = 50;
+	motorLeft.setSpeed(setpoint);
+	Time::wait_s(1);
+	motorLeft.setSpeed(0);
+	Time::wait_s(1);
+	log.close();
 #endif