Ahmad Fadhilah / Astro_RTOS_SUMO_backup

sumo

Dependencies:   ESC Servo mbed-dev mbed-rtos

main.cpp@0:98cd95fbb541, 2017-09-06 (annotated)

Committer:
astroboy1611
Date:
Wed Sep 06 12:47:03 2017 +0000
Revision:
0:98cd95fbb541
Original Sumo Program

Who changed what in which revision?

UserRevisionLine numberNew contents of line
astroboy1611 0:98cd95fbb541 1
astroboy1611 0:98cd95fbb541 2 #include "mbed.h"
astroboy1611 0:98cd95fbb541 3 #include "rtos.h"
astroboy1611 0:98cd95fbb541 4 #include "esc.h"
astroboy1611 0:98cd95fbb541 5 #include "Servo.h"
astroboy1611 0:98cd95fbb541 6
astroboy1611 0:98cd95fbb541 7 #define RightIR A3
astroboy1611 0:98cd95fbb541 8 #define LeftIR A1
astroboy1611 0:98cd95fbb541 9 #define RearIR A2
astroboy1611 0:98cd95fbb541 10
astroboy1611 0:98cd95fbb541 11 #define SERVO1 PB_6 //D10
astroboy1611 0:98cd95fbb541 12 #define SERVO2 PC_7 //D9
astroboy1611 0:98cd95fbb541 13
astroboy1611 0:98cd95fbb541 14 #define MotorLeft SERVO1
astroboy1611 0:98cd95fbb541 15 #define MotorRight SERVO2
astroboy1611 0:98cd95fbb541 16
astroboy1611 0:98cd95fbb541 17 #define BtnStart A5
astroboy1611 0:98cd95fbb541 18 #define BtnCalib A4
astroboy1611 0:98cd95fbb541 19
astroboy1611 0:98cd95fbb541 20 #define LeftBit 2
astroboy1611 0:98cd95fbb541 21 #define MidBit 1
astroboy1611 0:98cd95fbb541 22 #define RightBit 0
astroboy1611 0:98cd95fbb541 23
astroboy1611 0:98cd95fbb541 24
astroboy1611 0:98cd95fbb541 25 const double threshold = 0.2;
astroboy1611 0:98cd95fbb541 26
astroboy1611 0:98cd95fbb541 27 Serial pc(SERIAL_TX, SERIAL_RX);
astroboy1611 0:98cd95fbb541 28
astroboy1611 0:98cd95fbb541 29 DigitalOut led(LED1);
astroboy1611 0:98cd95fbb541 30
astroboy1611 0:98cd95fbb541 31 DigitalIn StartButton(BtnStart, PullUp);
astroboy1611 0:98cd95fbb541 32 DigitalIn CalibButton(BtnCalib, PullUp);
astroboy1611 0:98cd95fbb541 33
astroboy1611 0:98cd95fbb541 34 AnalogIn LeftSensor(LeftIR);
astroboy1611 0:98cd95fbb541 35 AnalogIn RightSensor(RightIR);
astroboy1611 0:98cd95fbb541 36 AnalogIn RearSensor(RearIR);
astroboy1611 0:98cd95fbb541 37
astroboy1611 0:98cd95fbb541 38 Servo LeftESC(MotorLeft);
astroboy1611 0:98cd95fbb541 39 Servo RightESC(MotorRight);
astroboy1611 0:98cd95fbb541 40
astroboy1611 0:98cd95fbb541 41 volatile double SensorRightVal, SensorLeftVal, SensorRearVal;
astroboy1611 0:98cd95fbb541 42 volatile unsigned int SensorFlag;
astroboy1611 0:98cd95fbb541 43
astroboy1611 0:98cd95fbb541 44 void initMotor()
astroboy1611 0:98cd95fbb541 45 {
astroboy1611 0:98cd95fbb541 46 LeftESC=0.5;
astroboy1611 0:98cd95fbb541 47 RightESC=0.5;
astroboy1611 0:98cd95fbb541 48 }
astroboy1611 0:98cd95fbb541 49 void setMotor(double L, double R)
astroboy1611 0:98cd95fbb541 50 {
astroboy1611 0:98cd95fbb541 51 L /=(double)2;
astroboy1611 0:98cd95fbb541 52 L+=(double)0.5;
astroboy1611 0:98cd95fbb541 53 R /=(double)2;
astroboy1611 0:98cd95fbb541 54 R+=(double)0.5;
astroboy1611 0:98cd95fbb541 55
astroboy1611 0:98cd95fbb541 56 LeftESC=L;
astroboy1611 0:98cd95fbb541 57 RightESC=R;
astroboy1611 0:98cd95fbb541 58 Thread::wait(20);
astroboy1611 0:98cd95fbb541 59 }
astroboy1611 0:98cd95fbb541 60 void SensorReader(void const *argument)
astroboy1611 0:98cd95fbb541 61 {
astroboy1611 0:98cd95fbb541 62 while(true)
astroboy1611 0:98cd95fbb541 63 {
astroboy1611 0:98cd95fbb541 64 SensorRightVal = RightSensor.read();
astroboy1611 0:98cd95fbb541 65 SensorLeftVal = LeftSensor.read();
astroboy1611 0:98cd95fbb541 66 SensorRearVal = RearSensor.read();
astroboy1611 0:98cd95fbb541 67 if(SensorRightVal>threshold)
astroboy1611 0:98cd95fbb541 68 {
astroboy1611 0:98cd95fbb541 69 SensorFlag |=(1<<RightBit);
astroboy1611 0:98cd95fbb541 70 }
astroboy1611 0:98cd95fbb541 71 else
astroboy1611 0:98cd95fbb541 72 {
astroboy1611 0:98cd95fbb541 73 SensorFlag &=~(1<<RightBit);
astroboy1611 0:98cd95fbb541 74 }
astroboy1611 0:98cd95fbb541 75 if(SensorLeftVal>threshold)
astroboy1611 0:98cd95fbb541 76 {
astroboy1611 0:98cd95fbb541 77 SensorFlag |=(1<<LeftBit);
astroboy1611 0:98cd95fbb541 78 }
astroboy1611 0:98cd95fbb541 79 else
astroboy1611 0:98cd95fbb541 80 {
astroboy1611 0:98cd95fbb541 81 SensorFlag &=~(1<<LeftBit);
astroboy1611 0:98cd95fbb541 82 }
astroboy1611 0:98cd95fbb541 83 if (SensorRearVal>threshold)
astroboy1611 0:98cd95fbb541 84 {
astroboy1611 0:98cd95fbb541 85 SensorFlag |=(1<<MidBit);
astroboy1611 0:98cd95fbb541 86 }
astroboy1611 0:98cd95fbb541 87 else
astroboy1611 0:98cd95fbb541 88 {
astroboy1611 0:98cd95fbb541 89 SensorFlag &=~(1<<MidBit);
astroboy1611 0:98cd95fbb541 90 }
astroboy1611 0:98cd95fbb541 91 Thread::wait(20); //20ms
astroboy1611 0:98cd95fbb541 92 }
astroboy1611 0:98cd95fbb541 93 }
astroboy1611 0:98cd95fbb541 94 void DataReporter(void const *argument)
astroboy1611 0:98cd95fbb541 95 {
astroboy1611 0:98cd95fbb541 96 while(true)
astroboy1611 0:98cd95fbb541 97 {
astroboy1611 0:98cd95fbb541 98 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());
astroboy1611 0:98cd95fbb541 99 Thread::wait(500);
astroboy1611 0:98cd95fbb541 100 }
astroboy1611 0:98cd95fbb541 101
astroboy1611 0:98cd95fbb541 102 }
astroboy1611 0:98cd95fbb541 103 void MotorController(void const *argument)
astroboy1611 0:98cd95fbb541 104 {
astroboy1611 0:98cd95fbb541 105 while(true)
astroboy1611 0:98cd95fbb541 106 {
astroboy1611 0:98cd95fbb541 107 switch(SensorFlag)
astroboy1611 0:98cd95fbb541 108 {
astroboy1611 0:98cd95fbb541 109 case 0b000:
astroboy1611 0:98cd95fbb541 110 setMotor(0.25,-0.25);
astroboy1611 0:98cd95fbb541 111 break;
astroboy1611 0:98cd95fbb541 112 case 0b001:
astroboy1611 0:98cd95fbb541 113 setMotor(0.25,-0.25);
astroboy1611 0:98cd95fbb541 114 break;
astroboy1611 0:98cd95fbb541 115 case 0b010:
astroboy1611 0:98cd95fbb541 116 setMotor(0.5,-0.5);
astroboy1611 0:98cd95fbb541 117 break;
astroboy1611 0:98cd95fbb541 118 case 0b011:
astroboy1611 0:98cd95fbb541 119
astroboy1611 0:98cd95fbb541 120 break;
astroboy1611 0:98cd95fbb541 121 case 0b100:
astroboy1611 0:98cd95fbb541 122 setMotor(-0.25,0.25);
astroboy1611 0:98cd95fbb541 123 break;
astroboy1611 0:98cd95fbb541 124 case 0b101:
astroboy1611 0:98cd95fbb541 125 setMotor(1,1);
astroboy1611 0:98cd95fbb541 126 break;
astroboy1611 0:98cd95fbb541 127 case 0b110:
astroboy1611 0:98cd95fbb541 128
astroboy1611 0:98cd95fbb541 129 break;
astroboy1611 0:98cd95fbb541 130 case 0b111:
astroboy1611 0:98cd95fbb541 131
astroboy1611 0:98cd95fbb541 132 break;
astroboy1611 0:98cd95fbb541 133
astroboy1611 0:98cd95fbb541 134 }
astroboy1611 0:98cd95fbb541 135 Thread::wait(1);
astroboy1611 0:98cd95fbb541 136 }
astroboy1611 0:98cd95fbb541 137 }
astroboy1611 0:98cd95fbb541 138 void enterCalibrationMode(void)
astroboy1611 0:98cd95fbb541 139 {
astroboy1611 0:98cd95fbb541 140 pc.printf("Entering Calibration Mode...\r\n");
astroboy1611 0:98cd95fbb541 141 pc.printf("Calib Button is Full Throttle\r\n");
astroboy1611 0:98cd95fbb541 142 pc.printf("Start Button is Full Reverse\r\n");
astroboy1611 0:98cd95fbb541 143 pc.printf("Please Restart Nucleo to exit from this mode\r\n");
astroboy1611 0:98cd95fbb541 144 while(true)
astroboy1611 0:98cd95fbb541 145 {
astroboy1611 0:98cd95fbb541 146 //pc.printf("entering loop\r\n");
astroboy1611 0:98cd95fbb541 147 if((CalibButton.read()==0)&&(StartButton.read()==1)) //full throttle
astroboy1611 0:98cd95fbb541 148 {
astroboy1611 0:98cd95fbb541 149 pc.printf("FullThrottle\r\n");
astroboy1611 0:98cd95fbb541 150 LeftESC=1;
astroboy1611 0:98cd95fbb541 151 RightESC=1;
astroboy1611 0:98cd95fbb541 152 }
astroboy1611 0:98cd95fbb541 153 else if ((StartButton.read()==0)&&(CalibButton.read()==1)) //Full Reverse
astroboy1611 0:98cd95fbb541 154 {
astroboy1611 0:98cd95fbb541 155 pc.printf("FullReverse\r\n");
astroboy1611 0:98cd95fbb541 156 LeftESC=0;
astroboy1611 0:98cd95fbb541 157 RightESC=0;
astroboy1611 0:98cd95fbb541 158 }
astroboy1611 0:98cd95fbb541 159 else //Neutral
astroboy1611 0:98cd95fbb541 160 {
astroboy1611 0:98cd95fbb541 161 pc.printf("Neutral\r\n");
astroboy1611 0:98cd95fbb541 162 LeftESC=0.5;
astroboy1611 0:98cd95fbb541 163 RightESC=0.5;
astroboy1611 0:98cd95fbb541 164 }
astroboy1611 0:98cd95fbb541 165 wait(0.1);
astroboy1611 0:98cd95fbb541 166 }
astroboy1611 0:98cd95fbb541 167 }
astroboy1611 0:98cd95fbb541 168 int main(void)
astroboy1611 0:98cd95fbb541 169 {
astroboy1611 0:98cd95fbb541 170 led=1;
astroboy1611 0:98cd95fbb541 171 initMotor();
astroboy1611 0:98cd95fbb541 172 pc.baud(115200);
astroboy1611 0:98cd95fbb541 173 Thread::wait(500);
astroboy1611 0:98cd95fbb541 174 pc.printf("*** RTOS Sumo ROBOT SMADA ***\r\n");
astroboy1611 0:98cd95fbb541 175 pc.printf("Press Calib button then power on to enter Calibration Mode\r\n");
astroboy1611 0:98cd95fbb541 176 if(CalibButton.read()==0)
astroboy1611 0:98cd95fbb541 177 {
astroboy1611 0:98cd95fbb541 178 pc.printf("Please Release Calib Button\r\n");
astroboy1611 0:98cd95fbb541 179 while(CalibButton.read()==0);
astroboy1611 0:98cd95fbb541 180 enterCalibrationMode();
astroboy1611 0:98cd95fbb541 181 }
astroboy1611 0:98cd95fbb541 182 pc.printf("Release Start button To begin the match...\r\n ");
astroboy1611 0:98cd95fbb541 183 while(StartButton.read()!=0);
astroboy1611 0:98cd95fbb541 184 Thread::wait(5000);
astroboy1611 0:98cd95fbb541 185 pc.printf("Bismillah.. Semoga kita menang, Amiinn...\r\n");
astroboy1611 0:98cd95fbb541 186 Thread SensorThread(SensorReader, NULL, osPriorityNormal, DEFAULT_STACK_SIZE);
astroboy1611 0:98cd95fbb541 187 Thread DataThread(DataReporter, NULL, osPriorityNormal, DEFAULT_STACK_SIZE);
astroboy1611 0:98cd95fbb541 188 Thread MotorThread(MotorController, NULL, osPriorityNormal, DEFAULT_STACK_SIZE);
astroboy1611 0:98cd95fbb541 189 while(1)
astroboy1611 0:98cd95fbb541 190 {
astroboy1611 0:98cd95fbb541 191 led=0;
astroboy1611 0:98cd95fbb541 192 Thread::wait(100);
astroboy1611 0:98cd95fbb541 193 led=1;
astroboy1611 0:98cd95fbb541 194 Thread::wait(100);
astroboy1611 0:98cd95fbb541 195 if(CalibButton.read()==0)
astroboy1611 0:98cd95fbb541 196 {
astroboy1611 0:98cd95fbb541 197 SensorThread.terminate();
astroboy1611 0:98cd95fbb541 198 DataThread.terminate();
astroboy1611 0:98cd95fbb541 199 MotorThread.terminate();
astroboy1611 0:98cd95fbb541 200 setMotor(0,0);
astroboy1611 0:98cd95fbb541 201 while(1);
astroboy1611 0:98cd95fbb541 202 }
astroboy1611 0:98cd95fbb541 203 }
astroboy1611 0:98cd95fbb541 204 }