Ahmad Fadhilah
sumo
Dependencies: ESC Servo mbed-dev mbed-rtos
Revision 0:98cd95fbb541, committed 2017-09-06
- Comitter:
- astroboy1611
- Date:
- Wed Sep 06 12:47:03 2017 +0000
- Commit message:
- Original Sumo Program
Changed in this revision
diff -r 000000000000 -r 98cd95fbb541 ESC.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/ESC.lib Wed Sep 06 12:47:03 2017 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/MatteoT/code/ESC/#735702ea5519
diff -r 000000000000 -r 98cd95fbb541 Servo.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/Servo.lib Wed Sep 06 12:47:03 2017 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/simon/code/Servo/#36b69a7ced07
diff -r 000000000000 -r 98cd95fbb541 main.cpp
--- /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);
+ }
+ }
+}
diff -r 000000000000 -r 98cd95fbb541 mbed-dev.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed-dev.lib Wed Sep 06 12:47:03 2017 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed-dev/#21d94c44109e
diff -r 000000000000 -r 98cd95fbb541 mbed-rtos.lib --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed-rtos.lib Wed Sep 06 12:47:03 2017 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed-rtos/#3da5f554d8bf