Diff: main.cpp

Revision:

2:684d5cf2f683

Parent:

0:6c0f87177797

Child:

3:98ea6afa0cf2

--- a/main.cpp	Mon Oct 16 13:52:31 2017 +0000
+++ b/main.cpp	Mon Oct 23 10:57:11 2017 +0000
@@ -1,51 +1,248 @@
 #include "mbed.h"
 #include "QEI.h"
-AnalogIn PotMeter(A0);
+#include "PID.h"
+#include "ReceiverIR.h"
+#include "Servo.h"
+
+ReceiverIR ir_rx(D2);
+
+AnalogIn PotMeter1(A0);
+AnalogIn PotMeter2(A1);
 InterruptIn Button(PTA4);
  
 PwmOut MotorThrottle1(D5);
 PwmOut MotorThrottle2(D6);
 DigitalOut MotorDirection1(D4);
 DigitalOut MotorDirection2(D7);
+DigitalOut servo(D3);
 
-QEI Encoder(D12,D13,NC,32);
+DigitalIn EndSwitch1(D9);
+DigitalIn EndSwitch2(D8);
+
+//DigitalOut Magneet(D7);
+
+
+QEI EncoderMotor1(D12,D13,NC,4200);
+QEI EncoderMotor2(D10,D11,NC,4200);
+
+Ticker ControlTicker;
+Ticker GetRefTicker;
+
+float Interval=0.02f;
+float pi=3.14159268;
+PID controller1(20.0f,0.0f,0.001f, Interval);
+PID controller2(20.0f,0.0f,0.002f, Interval);
+
+RemoteIR::Format format;
+uint8_t buf[4];
 
 Serial pc(PTB17,PTB16);
 
-void ButtonPress(){
-    MotorDirection1 = !MotorDirection1;
+float PosRef1=1.0f;
+float PosRef2=1.0f;
+
+
+
+
+void HomingLoop(){
+
+    EndSwitch1.mode(PullUp);
+    EndSwitch2.mode(PullUp);
+    
+    MotorThrottle1=0.1f;
+    MotorThrottle2=0.2f;
+    
+    MotorDirection1=1;
+    MotorDirection2=1;
+    
+    bool dummy1=0;
+    bool dummy2=0;
+    
+    while(EndSwitch1.read()|EndSwitch2.read()){
+        if((EndSwitch1.read()==0)&&(dummy1==0)){
+            MotorThrottle1=0.0f;
+            dummy1=1;
+        }
+        if((EndSwitch2.read()==0)&&(dummy2==0)){
+            MotorThrottle2=0.0f;
+            dummy2=1;
+        }
+    }
+    MotorThrottle1=0.0f;
+    MotorThrottle2=0.0f;
+    EncoderMotor1.reset();
+    EncoderMotor2.reset();
+}
+
+void ControlLoop(){
+    
+    float Pos1 = EncoderMotor1.getPulses()/4200.0f*2.0f*pi; //Motorpos. in radians
+    float Pos2 = EncoderMotor2.getPulses()/4200.0f*2.0f*pi; //Motorpos. in radians
+    float error1 = PosRef1 - Pos1;
+    float error2 = PosRef2 - Pos2;
+    
+    // PID input
+    controller1.setSetPoint(PosRef1);
+    controller2.setSetPoint(PosRef2);
+    
+    controller1.setProcessValue(Pos1);
+    controller2.setProcessValue(Pos2);
+    
+    // PID output
+    float OutPut1=controller1.compute();
+    float OutPut2=controller2.compute();
+
+    // Direction handling
+    float Direction1=0;
+    float Direction2=0;
+    
+    if(OutPut1<0){
+        Direction1=1;
+    }
+    if(OutPut2<0){
+        Direction2=1;
+    }
+    
+    OutPut1=fabs(OutPut1);
+    OutPut2=fabs(OutPut2);
+    
+    // Output schrijven
+    MotorThrottle1.write(OutPut1);
+    MotorThrottle2.write(OutPut2);
+    MotorDirection1=Direction1;
+    MotorDirection2=Direction2;
 }
 
 
+void PickUp(){
+
+}
+    
+void LayDown(){
+    
+    
+    }
+
+
+void RemoteController(){
+    
+   int bitcount;
+
+
+   bitcount = ir_rx.getData(&format, buf, sizeof(buf) * 8);
+    int state = buf[2];
+    switch(state)
+    {
+        case 22: //1
+            printf("1\n\r");
+        break;
+        case 25: //1
+            printf("2\n\r");
+            break;
+        case 13: //1                
+            printf("3\n\r");
+            break;
+        case 12: //1
+            printf("4\n\r");
+            break;
+        case 24: //1
+            printf("5\n\r");
+            ControlTicker.detach();
+            MotorThrottle1=0.0f;
+            MotorThrottle2=0.0f;
+            PickUp();
+            ControlTicker.attach(&ControlLoop, Interval);
+            break;
+        case 94: //1
+            printf("6\n\r");
+            break;
+        case 8: //1
+            printf("7\n\r");
+            break;
+        case 28: //1
+            printf("8\n\r");
+            ControlTicker.detach();
+            MotorThrottle1=0.0f;
+            MotorThrottle2=0.0f;
+            LayDown();
+            ControlTicker.attach(&ControlLoop, Interval);
+            break;
+        case 90: //1
+            printf("9\n\r");
+            break;
+        case 70: //1
+            pc.printf("Boven\n\r");
+            PosRef2=PosRef2-0.1f;
+            pc.printf("%f\n\r", PosRef2);
+            break;
+        case 21: //1
+            pc.printf("Onder\n\r");
+            PosRef2=PosRef2+0.1f;  
+            pc.printf("%f\n\r", PosRef2);
+            break;
+        case 68: //1
+            pc.printf("Links\n\r");
+            PosRef1=PosRef1+0.1f;
+            pc.printf("%f\n\r", PosRef1);
+            break;
+        case 67: //1
+            pc.printf("Rechts\n\r");
+            PosRef1=PosRef1-0.1f;
+            pc.printf("%f\n\r", PosRef1);
+            break;
+        case 64: //1
+            pc.printf("OK\n\r");
+            ControlTicker.detach();
+            MotorThrottle1=0.0f;
+            MotorThrottle2=0.0f;
+            HomingLoop();
+            ControlTicker.attach(&ControlLoop, Interval);
+            
+            break;
+        default:
+            break;
+    }
+}
+
+
+
+
+void DeterminePosRef(){
+    PosRef1=2*pi*PotMeter1.read(); // Reference in Rad
+    PosRef2=2*pi*PotMeter2.read(); // Reference in Rad
+}
+
 int main() {
     pc.baud(115200);
-    float speed = 0.0f;
-    int pos = 0;
-    int pos_ref = 0;
-    int temp_ref = 0;
-    int pos_prev = 0;
     pc.printf("startup");
-    Button.rise(&ButtonPress);
+    
+    controller1.setInputLimits(0.5f,2.0f*pi);
+    controller2.setInputLimits(0.5f,2.0f*pi);
+    controller1.setOutputLimits(-0.15f,0.15f);
+    controller2.setOutputLimits(-0.5f,0.5f);  
+    
+    
+    HomingLoop();
+    
     
-    while(true){
-        //pc.printf("PWM: %f, POS: %i, REF: %i\r\n",speed,pos,pos_ref);
-        pos = Encoder.getPulses();
-        temp_ref =(int) 10000*PotMeter.read();
-        if(fabsf(temp_ref-pos_ref)>50){
-            pos_ref = temp_ref;
+    ControlTicker.attach(&ControlLoop, Interval);
+    //GetRefTicker.attach(&DeterminePosRef, 0.1f);
+    
+
+    
+    while(1)
+    { 
+        if (ir_rx.getState() == ReceiverIR::Received)
+        {
+
+            pc.printf("received");
+            RemoteController();
+            wait(0.01);
         }
-        speed = ((float)(pos-pos_ref))/10000.0f;
-        if (speed<0.0f){
-            MotorDirection1=1;
-            speed = -speed;
-        }
-        else{
-            MotorDirection1=0;
-        }
-        if(speed<0.01f){
-            speed=0;
-        }
-        MotorThrottle1.write(speed);
-        pos_prev=pos;
-    }
+    }       
+    
+    
+    while(1){}
+
+
 }