Diff: main.cpp

Revision:

6:ee9361616596

Parent:

5:afe2339723f6

--- a/main.cpp	Sat Nov 18 20:42:14 2017 +0000
+++ b/main.cpp	Wed Dec 06 20:17:36 2017 +0000
@@ -9,9 +9,10 @@
 #include <sstream>
 
 //#define SERIAL
+#define BIAS 0.5
 #define ETHERNET
 
-#define SAMPLE_RATE 10 //sample rate in miliseconds
+#define SAMPLE_RATE 1 //sample rate in miliseconds
 
 enum status { IDLE, ADJUSTING, STABLE };
 
@@ -24,21 +25,25 @@
 HCSR04 ultrassonicSensor(PTC2, PTC3);
 Servo motor(PTA2);
 float motorPos = 0;
+int isRunning = 0;
 
 //Kc, Ti, Td, interval
-PID controller(1.0, 0.0, 0.0, SAMPLE_RATE);
+//PID controller(0.3461, 21.42, 5.26, 0.001);
+//PID controller(1.5, 0, 2, 0.001);
+//PID controller(-1.1, 0, 1.7, 0.001);//ok
+PID controller(-0.9344, 0, 1.733, 0.001);
 
 InterruptIn sw2(SW2);
 void sw2Callback()
 {
-    if(motorPos<1)
-        motorPos+=(float)0.1;
+    motorPos=0+BIAS;
+    isRunning = !isRunning;
 }
 InterruptIn sw3(SW3);
 void sw3Callback()
 {
-    if(motorPos>0)
-        motorPos-=(float)0.1;
+    /*if(motorPos>0)
+        motorPos-=(float)0.1;*/
 }
 
 Thread ledSwitchThread;
@@ -52,7 +57,7 @@
 #endif
 
 float ballDistance = 0.0;
-float setpoint = 15.0;
+float setpoint = 15;
 
 #ifdef ETHERNET
 
@@ -68,11 +73,11 @@
 
     while(true) {
         if(sock.is_connected()) {
-            sock.send_all(NULL,0);
+            sock.send_all("",1);
         } else {
             sock.connect("192.168.1.1", 12345);
         }
-        Thread::wait(100);
+        Thread::wait(1000);
     }
 }
 
@@ -107,7 +112,7 @@
 
 
         }
-        Thread::wait(1000);
+        Thread::wait(5000);
     }
 }
 
@@ -143,7 +148,7 @@
         } else {
             sock.connect("192.168.1.1", 12345);
         }
-        //Thread::wait(100);
+        Thread::wait(1000);
 
     }
 }
@@ -212,18 +217,36 @@
     printf("controlSystem thread started\n");
 #endif
     while(true) {
-        ballDistance = ultrassonicSensor.distance(CM);
+        ballDistance = ultrassonicSensor.distance(CM)+2.5;
+        
+        if (ballDistance > 37.5)
+            ballDistance = 35;
+        if (ballDistance <0)    
+            ballDistance = 2.5;
+            
+            
+        controller.setProcessValue(ballDistance);
 
         if (ballDistance != setpoint) {
             statusFlag = ADJUSTING;
-        } else {
+        } else if (ballDistance == setpoint){
             statusFlag = STABLE;
+        } else if (!isRunning){
+            statusFlag = IDLE;
         }
 
         //PID CONTROLLER
         //motor.write(motorPos);
-        controller.setProcessValue(ballDistance);
-        motor.write(controller.compute());
+        //controller.setProcessValue(ballDistance);
+        
+        controller.setSetPoint(setpoint);
+        if(isRunning)
+            motorPos = controller.compute();
+       
+       #ifdef SERIAL
+        //printf("Motor position: %f\n",motorPos);
+        #endif           
+        motor = 1-motorPos;
 
 
         Thread::wait(SAMPLE_RATE);
@@ -233,6 +256,21 @@
 
 int main()
 {
+    
+    
+    Servo myservo(PTA2);
+/*
+while(1) {    
+    for(float p=-90; p<=90; p += 45) {
+        myservo.position(180-p+BIAS);
+        wait(1);
+    }
+}
+*/
+    motor = 0.5;;
+    wait(1);
+    
+    
     statusFlag = IDLE;
 
 #ifdef SERIAL
@@ -240,11 +278,12 @@
     printf("APS de Sistemas Operacionais / Controle 2\n");
     printf("Alunos: Felipe, Juliana, Rafael\n");
 #endif
-    //Analog input from 0.0 to 50.0 cm
-    controller.setInputLimits(0.0, 50.0);
+    // input from 0.0 to 35 cm
+    controller.setInputLimits(-1, 50.0);
     //Pwm output from 0.0 to 1.0 (servo)
-    controller.setOutputLimits(0.0, 1.0);
+    controller.setOutputLimits(0, 1);
     //If there's a bias.
+    controller.setBias(BIAS);
     //controller.setBias(0.3);
     controller.setMode(AUTO_MODE);
     //We want the process variable to be 15cm (default)