Diff: main.cpp

Revision:

0:4d45673148c2

diff -r 000000000000 -r 4d45673148c2 main.cpp
--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Sun Dec 11 13:47:17 2016 +0000
@@ -0,0 +1,224 @@
+#include "mbed.h" 
+#include "Motor.h"
+#include "PID.h"
+#include <stdlib.h>
+#include "rtos.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;
+Serial xbee(p13, p14);
+Serial pc(USBTX,USBRX);
+Mutex wheelMutex;
+Mutex timermutex; 
+
+//functions
+void go_fwd(float);
+void turn_R(float);
+void turn_L(float);
+void stop(float);
+void tunePID(void);
+void go_fwd_thread(void const *argument);
+
+//globals:
+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
+bool go;
+float go_time = 0.0;
+
+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.1, 0.01);  //Kc, Ti, Td
+PID rightPid(0.4620, 0.1, 0.1, 0.01); //Kc, Ti, Td
+//PID rightPid(0.515, 0.1, 0.0, 0.01); //Kc, Ti, Td
+
+int main()
+{
+    tunePID();
+    xbee.baud(19200);
+    leftPid.setSetPoint(975);  //set velocity goals for PID
+    rightPid.setSetPoint(990);
+    wait(1);
+    Thread fwdThread(go_fwd_thread);
+    go = false;
+    char tag;
+    char turnDirectionBuf[5];
+    float turnDirection; 
+    char distToTravelBuf[5];
+    float distToTravel;
+    while(1)
+    {
+        tag = xbee.getc();
+        
+        if(tag == '*')
+        {
+            //pc.printf("tag=%c\r\n",tag);
+            distToTravelBuf[0] = xbee.getc();
+            distToTravelBuf[1] = xbee.getc();
+            distToTravelBuf[2] = xbee.getc();
+            distToTravelBuf[3] = xbee.getc();
+            distToTravelBuf[4] = xbee.getc();
+            //pc.printf("dist = %s\r\n",distToTravelBuf);
+            distToTravel = atof(distToTravelBuf);
+            //pc.printf("traveldist=%f\r\n",distToTravel);
+            //go_fwd(distToTravel);
+            timermutex.lock();
+            go_time = distToTravel;
+            timermutex.unlock();
+            go = true;
+                                    
+        }
+        else if(tag == 'R')
+        {
+            //pc.printf("tag=%c\r\n",tag);
+            turnDirectionBuf[0] = xbee.getc();
+            turnDirectionBuf[1] = xbee.getc();
+            turnDirectionBuf[2] = xbee.getc();
+            turnDirectionBuf[3] = xbee.getc();
+            turnDirectionBuf[4] = xbee.getc();
+            //pc.printf("turn right %s\n\r",turnDirectionBuf);
+            turnDirection = atof(turnDirectionBuf);
+            //pc.printf("turn right %f\n\r",turnDirection);
+            stop(1);
+            turn_R(turnDirection);
+        }
+        else if(tag == 'L')
+        {
+            //pc.printf("tag=%c\r\n",tag);
+            turnDirectionBuf[0] = xbee.getc();
+            turnDirectionBuf[1] = xbee.getc();
+            turnDirectionBuf[2] = xbee.getc();
+            turnDirectionBuf[3] = xbee.getc();
+            turnDirectionBuf[4] = xbee.getc();
+            //pc.printf("turn left %s\n\r",turnDirectionBuf);
+            turnDirection = atof(turnDirectionBuf);
+            //pc.printf("turn left %f\n\r",turnDirection);
+            stop(1);
+            turn_L(turnDirection);
+        }   
+    }
+} 
+
+void tunePID(void){
+    leftPid.setInputLimits(0, 3000);    
+    leftPid.setOutputLimits(0.0, 0.4);
+    leftPid.setMode(AUTO_MODE);
+    rightPid.setInputLimits(0, 3000);
+    rightPid.setOutputLimits(0.0, 0.4);
+    rightPid.setMode(AUTO_MODE);
+}
+
+//void go_fwd(float time)
+//{
+void go_fwd_thread(void const *argument) {
+    while(1){
+        if(!go) {Thread::yield();}
+        while (go) {
+            wheelMutex.lock();
+
+    timer.start();
+    while(timer.read_ms() < go_time){
+    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());
+    rightPid.setProcessValue(fabs(rightVelocity));
+    rightMotor.speed(rightPid.compute());
+    }
+    timer.stop();
+    timer.reset();
+    go = false;
+    timermutex.lock();
+    go_time = 0.0;
+    timermutex.unlock();
+    wheelMutex.unlock();
+    }
+ 
+  }
+}
+
+
+void stop(float t)
+{
+    wheelMutex.lock();
+    
+    leftMotor.speed(0.0);    
+    rightMotor.speed(0.0);
+    //wait(t);
+    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.35);  //rotate to the right
+    rightMotor.speed(-0.35);  //open loop, because the PID class can't handle negative values
+    leftActPulses=leftPulses.read();
+    rightActPulses=rightPulses.read();
+    wait(0.005);
+    }
+    wheelMutex.unlock();
+    stop(1);
+}
+    
+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.35);  //rotate to the right
+    rightMotor.speed(0.35);  //open loop, because the PID class can't handle negative values
+    leftActPulses=leftPulses.read();
+    rightActPulses=rightPulses.read();
+    wait(0.005);
+    }
+    wheelMutex.unlock();
+    stop(1);
+}
+
+
+