Diff: main.cpp

Revision:

0:35cf52223527

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sun Dec 11 13:37:12 2016 +0000
@@ -0,0 +1,316 @@
+#include "mbed.h" 
+#include "Servo.h"
+#include "Motor.h"
+#include "PID.h"
+#include "rtos.h"
+#include <stdlib.h>
+
+#define PI 3.1415926
+#define threshold 27
+
+
+//objects
+Motor rightMotor(p24, p28, p27); // pwm, fwd(in1), rev(in2) Motor B (right wheel)
+Motor leftMotor(p22, p29, p30); // pwm, fwd(in1), rev(in2) Motor A (left wheel)
+Timer timer;
+AnalogIn ain(p20);
+AnalogIn ainL(p18);
+AnalogIn ainR(p19);
+Servo myservo(p26);
+Serial pc(USBTX, USBRX);
+Serial xbee1(p13, p14);
+Mutex wheelMutex; 
+
+
+//globals
+float max_angle = 0.0;
+int leftPrevPulses  = 0;
+int leftActPulses=0; //pulses from left motor
+float leftVelocity  = 0.0; //The velocity of the left wheel in pulses per second
+int rightPrevPulses = 0;
+int rightActPulses=0; //pulses from right motor
+float rightVelocity = 0.0; //The velocity of the right wheel in pulses per second
+int turnDistance     = 0; //Number of pulses to travel.
+int distanceR = 0; // in mm
+int distanceL = 0; // in mm
+int distanceRold = 0; // in mm
+int distanceLold = 0; // in mm
+float X  = 0.0;
+float Y = 0.0;
+float inc_X = 1.0;
+float inc_Y = 0.0;
+float cur_dir = 0.0;
+bool go;
+int DistancesR[4];
+int DistancesL[4];
+
+
+//functions
+void lookaround(void);
+void go_fwd(void);
+void turn_R(float);
+void turn_L(float);
+void stop(void);
+void distR(int);
+void distL(int);
+int distTransform(float input);
+void tunePID(void);
+float find_avg(float a[5]);
+float to_Rad(float);
+void go_fwd_thread(void const *argument);
+void grab_init_direct(void);
+
+class Counter {     //interrupt driven rotation counter to be used with the feedback control
+    public:
+    Counter(PinName pin) : _interrupt(pin) {        // create the InterruptIn on the pin specified to Counter
+        _interrupt.rise(this, &Counter::increment); // attach increment function of this counter instance
+    }
+ 
+    void increment() {
+        _count++;
+    }
+ 
+    int read() {
+        return _count;
+    }
+ 
+private:
+    InterruptIn _interrupt;
+    volatile int _count;
+};
+
+Counter leftPulses(p9), rightPulses (p10);
+//PID leftPid(0.4620, 0.1, 0.0, 0.01);  //Kc, Ti, Td
+//PID rightPid(0.4620, 0.1, 0.0, 0.01); //Kc, Ti, Td
+
+PID leftPid(0.462, 0.1, 0.2, 0.01);  //Kc, Ti, Td
+PID rightPid(0.462, 0.1, 0.1, 0.01); //Kc, Ti, Td
+
+int main() 
+{
+    tunePID();
+    xbee1.baud(115200);
+    myservo = 0.65;
+    leftPid.setSetPoint(975);  //set velocity goals for PID
+    rightPid.setSetPoint(990);
+    Thread fwdThread(go_fwd_thread);
+    grab_init_direct();
+    go = true;
+    float middle[5];
+    float left[5];
+    float right[5];
+    
+    while(1){
+        float mid_avg = 0.0; 
+        float left_avg = 0.0;
+        float right_avg = 0.0;
+        timer.start();
+        while (mid_avg <0.4 && left_avg <0.7 && right_avg <0.4) 
+        {
+            middle[0] = ain;
+            middle[1] = ain;
+            middle[2] = ain;
+            middle[3] = ain;
+            middle[4] = ain;
+            mid_avg = find_avg(middle);
+            
+            left[0] = ainL;
+            left[1] = ainL;
+            left[2] = ainL;
+            left[3] = ainL;
+            left[4] = ainL;
+            left_avg = find_avg(left);
+            
+            right[0] = ainR;
+            right[1] = ainR;
+            right[2] = ainR;
+            right[3] = ainR;
+            right[4] = ainR;
+            right_avg = find_avg(right);
+            
+            X = X + inc_X;
+            Y = Y + inc_Y;
+           xbee1.printf("+\n");
+           xbee1.printf("%.2f\n%.2f\n" ,X ,Y);   
+        }
+        go = false;
+        stop();
+        xbee1.printf("*\n");
+        xbee1.printf("%4d",timer.read_ms());
+        timer.stop();
+        timer.reset();
+        lookaround();
+        //fwdThread.signal_set(0x1);
+        
+        if (max_angle >= 45.0) 
+        {
+            turn_R((max_angle - 45.0));            
+            cur_dir = cur_dir - (max_angle - 45.0);
+            inc_X = cos(to_Rad(cur_dir));
+            inc_Y = sin(to_Rad(cur_dir));
+            xbee1.printf("R\n");
+            xbee1.printf("%2.1f",(max_angle - 45.0));
+        }
+        else if (max_angle < 45.0) 
+        {
+            turn_L((45.0 - max_angle));
+            cur_dir = cur_dir + (45.0 - max_angle);
+            inc_X = cos(to_Rad(cur_dir));
+            inc_Y = sin(to_Rad(cur_dir));
+            xbee1.printf("L\n");
+            xbee1.printf("%2.1f",(45.0 - max_angle));
+        }
+        
+        myservo = 0.65;
+        stop();         
+        wait(0.1);
+        go = true;
+        
+    } // End of while(1)loop - main thread
+}// End of main()
+
+void grab_init_direct(void)
+{
+    char initdirect[4];
+    int Count = 0;
+    initdirect[Count] = xbee1.getc();
+    while(initdirect[Count] != '+')
+    {
+        Count++;
+        initdirect[Count] = xbee1.getc();
+    }
+    
+    char initdirect2[Count];
+    for(int i=0; i<Count; i++)
+    {
+        initdirect2[i] = initdirect[i];
+    }
+    
+    float init_turn = atof(initdirect2);
+    cur_dir = init_turn;
+    turn_R(init_turn);
+        
+}
+
+float find_avg(float a[5])
+{
+    return ((a[0] + a[1] + a[2] + a[3] + a[4])/5.0);
+}
+
+void tunePID(void){
+    leftPid.setInputLimits(0, 3000);    
+    leftPid.setOutputLimits(0.0, 0.5);
+    leftPid.setMode(AUTO_MODE);
+    rightPid.setInputLimits(0, 3000);
+    //rightPid.setOutputLimits(0.0, 0.465);
+    rightPid.setOutputLimits(0.0, 0.48);
+    rightPid.setMode(AUTO_MODE);
+}
+
+
+void stop(void)
+{
+    wheelMutex.lock();
+    
+    leftMotor.speed(0.0);    
+    rightMotor.speed(0.0);
+    
+    wheelMutex.unlock();
+}
+
+
+void turn_R(float deg)
+{
+    wheelMutex.lock();
+    
+    leftActPulses=leftPulses.read();
+    rightActPulses=rightPulses.read();
+    turnDistance=rightActPulses+(int)deg;     
+    while (rightActPulses<turnDistance) { //turn approximately half a revolution
+    leftMotor.speed(0.4);  //rotate to the right
+    rightMotor.speed(-0.4);  //open loop, because the PID class can't handle negative values
+    leftActPulses=leftPulses.read();
+    rightActPulses=rightPulses.read();
+    wait(0.005);
+    }
+    
+    wheelMutex.unlock();
+}
+    
+void turn_L(float deg)
+{
+    
+    wheelMutex.lock();
+    
+    leftActPulses=leftPulses.read();
+    rightActPulses=rightPulses.read();
+    turnDistance=leftActPulses+(int)deg;     
+    while (leftActPulses<turnDistance) { //turn approximately half a revolution
+    leftMotor.speed(-0.4);  //rotate to the right
+    rightMotor.speed(0.4);  //open loop, because the PID class can't handle negative values
+    leftActPulses=leftPulses.read();
+    rightActPulses=rightPulses.read();
+    wait(0.005);
+    }
+    
+    wheelMutex.unlock();
+}
+ 
+void lookaround(void) //returns a value between 0 and 90 that represents the best path in the cone of observation
+{
+    float max;
+    max_angle = 0.0;
+    max = ain;
+    for(float p=0; p<1.0; p += 0.05) 
+    {
+        myservo = p;
+        if (max > ain)
+            {
+                max = ain;
+                max_angle = p;
+            }
+            
+        //wait(0.25);
+        wait(0.1);
+    }
+    
+    if (max>0.45){
+        turn_L(180.0);
+        myservo = 0.65;
+        wait(0.5);
+        }
+    else{        
+    myservo = max_angle;
+    wait(0.5);
+    max_angle = max_angle*90.0 - 13.5;
+    //max_angle = max_angle*90.0;
+    }    
+   
+}
+
+float to_Rad(float deg)
+{
+ return deg * ((PI)/180);   
+}
+
+void go_fwd_thread(void const *argument) {
+    while(1){
+        if(!go) {Thread::yield();}
+        while (go) {
+            wheelMutex.lock();
+            
+            leftActPulses=leftPulses.read();
+            leftVelocity = (leftActPulses - leftPrevPulses) / 0.01;
+            leftPrevPulses = leftActPulses;
+            rightActPulses=rightPulses.read();
+            rightVelocity = (rightActPulses - rightPrevPulses) / 0.01;
+            rightPrevPulses = rightActPulses;    
+            leftPid.setProcessValue(fabs(leftVelocity));
+            leftMotor.speed(leftPid.compute()+0.0075);
+            rightPid.setProcessValue(fabs(rightVelocity));
+            rightMotor.speed(rightPid.compute());
+            
+            wheelMutex.unlock();
+        }
+    }
+}