Diff: main.cpp

Revision:

0:98cd95fbb541

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Wed Sep 06 12:47:03 2017 +0000
@@ -0,0 +1,204 @@
+
+#include "mbed.h"
+#include "rtos.h"
+#include "esc.h"
+#include "Servo.h"
+
+#define RightIR     A3
+#define LeftIR      A1
+#define RearIR      A2
+
+#define SERVO1      PB_6 //D10
+#define SERVO2      PC_7 //D9
+
+#define MotorLeft   SERVO1
+#define MotorRight  SERVO2
+
+#define BtnStart    A5
+#define BtnCalib    A4
+
+#define LeftBit     2
+#define MidBit      1
+#define RightBit    0
+
+
+const double threshold = 0.2;
+
+Serial pc(SERIAL_TX, SERIAL_RX);
+
+DigitalOut led(LED1);
+
+DigitalIn StartButton(BtnStart, PullUp);
+DigitalIn CalibButton(BtnCalib, PullUp);
+
+AnalogIn LeftSensor(LeftIR); 
+AnalogIn RightSensor(RightIR);
+AnalogIn RearSensor(RearIR);
+
+Servo LeftESC(MotorLeft);
+Servo RightESC(MotorRight);
+
+volatile double SensorRightVal, SensorLeftVal, SensorRearVal;
+volatile unsigned int SensorFlag;
+
+void initMotor()
+{
+    LeftESC=0.5;
+    RightESC=0.5;
+}
+void setMotor(double L, double R)
+{
+    L /=(double)2;
+    L+=(double)0.5;
+    R /=(double)2;
+    R+=(double)0.5;
+    
+    LeftESC=L;
+    RightESC=R;
+    Thread::wait(20);
+}
+void SensorReader(void const *argument)
+{
+    while(true)
+    {
+        SensorRightVal = RightSensor.read();
+        SensorLeftVal = LeftSensor.read();
+        SensorRearVal = RearSensor.read();
+        if(SensorRightVal>threshold)
+        {
+            SensorFlag |=(1<<RightBit);
+        }
+        else
+        {
+            SensorFlag &=~(1<<RightBit);
+        }
+        if(SensorLeftVal>threshold)
+        {
+            SensorFlag |=(1<<LeftBit);
+        }
+        else
+        {
+            SensorFlag &=~(1<<LeftBit);
+        }
+        if (SensorRearVal>threshold)
+        {
+            SensorFlag |=(1<<MidBit);
+        }
+        else
+        {
+            SensorFlag &=~(1<<MidBit);
+        }
+        Thread::wait(20); //20ms
+    }
+}
+void DataReporter(void const *argument)
+{
+   while(true)
+   {
+       pc.printf("Sensor Flag 0x%X L = %10.5f R = %10.5f B = %10.5f BtnStart %d BtnCalib %d\r\n", SensorFlag, LeftSensor.read(), RightSensor.read(), RearSensor.read(), StartButton.read(), CalibButton.read());
+       Thread::wait(500);
+   }
+       
+}
+void MotorController(void const *argument)
+{
+    while(true)
+    {
+        switch(SensorFlag)
+        {
+            case 0b000:
+                setMotor(0.25,-0.25);
+            break;
+            case 0b001:
+                setMotor(0.25,-0.25);
+            break;
+            case 0b010:
+                setMotor(0.5,-0.5);
+            break;
+            case 0b011:
+                
+            break;
+            case 0b100:
+                setMotor(-0.25,0.25);
+            break;
+            case 0b101:
+                setMotor(1,1);
+            break;
+            case 0b110:
+                
+            break;
+            case 0b111:
+                
+            break;
+               
+        }
+        Thread::wait(1);
+    }
+}
+void enterCalibrationMode(void)
+{
+    pc.printf("Entering Calibration Mode...\r\n");
+    pc.printf("Calib Button is Full Throttle\r\n");
+    pc.printf("Start Button is Full Reverse\r\n");
+    pc.printf("Please Restart Nucleo to exit from this mode\r\n");
+    while(true)
+    {
+        //pc.printf("entering loop\r\n");
+        if((CalibButton.read()==0)&&(StartButton.read()==1)) //full throttle
+        {
+            pc.printf("FullThrottle\r\n");
+            LeftESC=1;
+            RightESC=1;
+        }
+        else if ((StartButton.read()==0)&&(CalibButton.read()==1)) //Full Reverse
+        {
+            pc.printf("FullReverse\r\n");
+            LeftESC=0;
+            RightESC=0;
+        }
+        else //Neutral
+        {
+            pc.printf("Neutral\r\n");
+            LeftESC=0.5;
+            RightESC=0.5;
+        }
+        wait(0.1);
+    }
+}
+int main(void)
+{
+    led=1;
+    initMotor();
+    pc.baud(115200);
+    Thread::wait(500);
+    pc.printf("*** RTOS Sumo ROBOT SMADA ***\r\n");
+    pc.printf("Press Calib button then power on to enter Calibration Mode\r\n");
+    if(CalibButton.read()==0)
+    {
+        pc.printf("Please Release Calib Button\r\n");
+        while(CalibButton.read()==0);
+        enterCalibrationMode();
+    }
+    pc.printf("Release Start button To begin the match...\r\n ");
+    while(StartButton.read()!=0);
+    Thread::wait(5000);
+    pc.printf("Bismillah.. Semoga kita menang, Amiinn...\r\n");
+    Thread SensorThread(SensorReader, NULL, osPriorityNormal, DEFAULT_STACK_SIZE);
+    Thread DataThread(DataReporter, NULL, osPriorityNormal, DEFAULT_STACK_SIZE);
+    Thread MotorThread(MotorController, NULL, osPriorityNormal, DEFAULT_STACK_SIZE);
+    while(1)
+    {
+       led=0;
+       Thread::wait(100);
+       led=1;
+       Thread::wait(100);
+       if(CalibButton.read()==0)
+       {
+           SensorThread.terminate();
+           DataThread.terminate();
+           MotorThread.terminate();
+           setMotor(0,0);
+           while(1);
+        }
+    }
+}