Leader Robot

Dependencies:   Motor PID Servo mbed-rtos mbed

Files at this revision

API Documentation at this revision

Comitter:
mpereira30
Date:
Sun Dec 11 13:37:12 2016 +0000
Commit message:
Final Project Leader Robot

Changed in this revision

Motor.lib Show annotated file Show diff for this revision Revisions of this file
PID.lib Show annotated file Show diff for this revision Revisions of this file
Servo.lib Show annotated file Show diff for this revision Revisions of this file
main.cpp Show annotated file Show diff for this revision Revisions of this file
mbed-rtos.lib Show annotated file Show diff for this revision Revisions of this file
mbed.bld Show annotated file Show diff for this revision Revisions of this file
diff -r 000000000000 -r 35cf52223527 Motor.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Motor.lib	Sun Dec 11 13:37:12 2016 +0000
@@ -0,0 +1,1 @@
+https://developer.mbed.org/users/mpereira30/code/Motor/#d76429c5c280
diff -r 000000000000 -r 35cf52223527 PID.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/PID.lib	Sun Dec 11 13:37:12 2016 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/aberk/code/PID/#6e12a3e5af19
diff -r 000000000000 -r 35cf52223527 Servo.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/Servo.lib	Sun Dec 11 13:37:12 2016 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/simon/code/Servo/#36b69a7ced07
diff -r 000000000000 -r 35cf52223527 main.cpp
--- /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();
+        }
+    }
+}
diff -r 000000000000 -r 35cf52223527 mbed-rtos.lib
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed-rtos.lib	Sun Dec 11 13:37:12 2016 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed-rtos/#b744dfee1cf2
diff -r 000000000000 -r 35cf52223527 mbed.bld
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed.bld	Sun Dec 11 13:37:12 2016 +0000
@@ -0,0 +1,1 @@
+http://mbed.org/users/mbed_official/code/mbed/builds/d75b3fe1f5cb
\ No newline at end of file