Versão entregue

Lab Encoder/src/main.cpp

@@ -11,38 +11,9 @@ uint8_t state = 0;
 uint8_t lastState = 0;
 
 int32_t theta = 0;
-int32_t targetTheta = 100;
 
-int32_t error = 0;
-int32_t lastError = 0;
-int32_t dError = 0;
-int32_t iError = 0;
-int32_t lastInstant = 0;
-int32_t instant;
-float Kp = 10;
-float Kd = 0;
-float Ki = 0;
-
-bool f = false;
-
-int32_t period = 10000;
-
-void update() {
-  delay(1);
-  lastInstant = instant;
-  instant = micros();
-  int32_t deltaT = instant - lastInstant;
-  lastError = error;
-  error = targetTheta - theta;
-  dError = (error - lastError)/deltaT;
-  iError = iError + error*deltaT;
-  if (iError > 255)
-    iError = 255;
-  else if (iError < -255)
-    iError = -255;
-}
-
-int16_t pid() { return Kp * error + Ki * iError + Kd * dError; }
+uint8_t buffer[3];
+int16_t power;
 
 void setPower(int16_t power) {
   if (power > 255) {
@@ -63,12 +34,10 @@ void setPower(int16_t power) {
 }
 
 void plot(){
-  Serial.print(">timestamp:");
+  Serial.print(">t:");
   Serial.println(micros());
   Serial.print(">theta:");
   Serial.println(theta);
-  Serial.print(">targetTheta:");
-  Serial.println(targetTheta);
 }
 
 void encoderIsr() {
@@ -104,6 +73,7 @@ void encoderIsr() {
     }
     break;
   }
+  plot();
 }
 
 void setup() {
@@ -119,15 +89,11 @@ void setup() {
 }
 
 void loop() {
-  if (!f && millis() % period < period/2) {
-    targetTheta = 100;
-    f = true;
-  } else if (f && millis() % period > period/2) {
-    targetTheta = -100;
-    f = false;
+
+  if(Serial.available() >= 7){
+    Serial.readBytesUntil('\n', buffer, 7);
+    sscanf((const char*)buffer, "0x%04X", &power);
+    setPower(power);
   }
-  plot();
-  update();
-  setPower(pid());
-  delay(10);
+  delay(100);
 }