Diff: main.cpp

Revision:

0:354a8831107d

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Dec 03 22:33:03 2014 +0000
@@ -0,0 +1,157 @@
+#include "mbed.h"
+#include "rtos.h"
+#include "Robot.h"
+
+DigitalOut led1(LED1);
+DigitalOut led2(LED2);
+DigitalOut led3(LED3);
+DigitalOut led4(LED4);
+
+//RFID data from other MBed
+DigitalIn rfIdBit1(p21);
+DigitalIn rfIdBit0(p22);
+InterruptIn rfIdInterrupt(p23);
+
+//Robot code
+Serial device(p9, p10);
+Serial rob(p13, p14);
+Robot TheRobot(&rob, &led1);
+
+
+
+//Debug
+Serial pc(USBTX, USBRX); // tx, rx
+Mutex pc_mutex;
+
+//Global Variables
+float biasRate = 0.1;
+float updateRate = 2.0;
+float speed_modifier=1;
+float current_speed=0;
+int p1xdir=1,p2xdir=-1; //Direction
+int speed_limit=0;
+
+void limit_detect(void const *args)
+{
+    while(1)
+    {
+        if(current_speed<speed_limit) speed_modifier=1.0;
+        else speed_modifier = speed_limit / current_speed;
+        Thread::wait(20);
+    }
+}
+
+void SetSpeed(void const *args)
+{
+   while(1)
+   {
+      //Set bits
+      int  RfidFlag = 0;
+      int V1 = rfIdBit1? 1:0;
+      int V0 = rfIdBit0? 1:0;
+
+      //Set flag
+      RfidFlag = V1 <<1;
+      RfidFlag |= V0;
+      if(RfidFlag == 0) continue;
+  
+      //Debug
+      //pc_mutex.lock();
+      pc.printf("%d , %d    ___ %d\n",rfIdBit1? 1:0, rfIdBit0?1:0, RfidFlag);
+      //pc_mutex.unlock();
+   
+      //Set speed limit based on RfidFlag
+      switch(RfidFlag)
+      {
+          case 0:
+              speed_limit =  0;
+              break;
+          case 1:
+              speed_limit =  10;
+              p1xdir = 1;
+              p2xdir = -1;
+              break;
+          case 2:
+              speed_limit = 10;
+              p1xdir = -1;
+              p2xdir = 1;
+              break;
+          case 3:
+              speed_limit = 60;
+              p1xdir = 1;
+              p2xdir = 1;
+              break;
+          default:
+              break;
+           
+    }
+    TheRobot.SetSelectMotorVelocity(p1xdir*speed_limit, p2xdir*speed_limit);
+
+    Thread::wait(500);
+   }
+}
+
+int main() 
+{
+    //Start thread
+    Thread limit_change(limit_detect);
+    //Thread Poll_IMU(CalcSpeed);
+    Thread Poll_RFID(SetSpeed);
+
+    int JS1x=0, JS2x=0; //Translated binary values from joystick
+    int p1x=0,p2x=0; //PWM power
+    char txchar=0x00;
+    char txflag=0x00;
+    device.baud(600);
+    //TheRobot.SetMotorVelocity(30);
+    while(1) 
+    {
+        txchar=device.getc();
+        txflag=txchar & 0xC0;
+        switch(txflag)
+        {
+            case 0x00:
+                JS1x=(int)txchar & 0x3F;
+                break;
+            case 0xC0:
+                JS2x=(int)txchar & 0x3F;
+                break;
+            default:
+                break;
+        }
+        
+    
+    
+        //Break info into direction and magnitude
+        //if(JS1x>31){p1xdir=-1; p1x=JS1x-32;}
+        //else {p1xdir=1; p1x=31-JS1x;}
+        //if(JS2x>31){p2xdir=1; p2x=JS2x-32;}
+        //else {p2xdir=-1; p2x=31-JS2x;}
+        
+        
+        current_speed=sqrt((float)p1x*(float)p1x+(float)p2x*(float)p2x);
+        
+        
+        //PMW output based on modifier
+        //pmwL = p1xdir * (speed_modifier * p1x) * 2;
+        //pmwR = p2xdir * (speed_modifier * p2x) * 2;
+        
+        //TheRobot.SetSelectMotorVelocity(pmwL, pmwR);
+        //TheRobot.WaitForAck();
+        //TheRobot.SetSelectMotorVelocity(2, 60);
+        //wait(5);
+        //TheRobot.SetSelectMotorVelocity(1, 15);
+        //wait(5);
+        //TheRobot.SetSelectMotorVelocity(2, pmwR);
+        
+        //DEBUG
+        //if(JS1x>16) led1=1;
+        //else led1=0;
+        /*if(JS1x>32) led2=1;
+        else led2=0;
+        if(JS2x>16) led3=1;
+        else led3=0;
+        if(JS2x>32) led4=1;
+        else led4=0;*/
+    }
+}