KRTMI 2019 / Mbed 2 deprecated KRTMI_v1

code asli

Dependencies:   mbed Servo encoderKRTMI Motornew Lengan3 millis

main.cpp@2:ab055f0284fc, 2019-02-24 (annotated)

Committer:
SalbiFaza
Date:
Sun Feb 24 09:45:32 2019 +0000
Revision:
2:ab055f0284fc
Child:
3:6d469c371f49
bismillah

Who changed what in which revision?

UserRevisionLine numberNew contents of line
SalbiFaza 2:ab055f0284fc 1 #include "JoystickPS3.h"
SalbiFaza 2:ab055f0284fc 2 #include "Motor.h"
SalbiFaza 2:ab055f0284fc 3 #include "mbed.h"
SalbiFaza 2:ab055f0284fc 4 /* Inisialisasi Pin TX-RX Joystik dan PC */
SalbiFaza 2:ab055f0284fc 5 joysticknucleo joystick(PA_0,PA_1);
SalbiFaza 2:ab055f0284fc 6 Serial pc(USBTX,USBRX);
SalbiFaza 2:ab055f0284fc 7 /* Deklarasi Motor Base */
SalbiFaza 2:ab055f0284fc 8 Motor motorDepan(PB_7, PC_3, PC_0); //(PB_9, PA_12, PC_5);
SalbiFaza 2:ab055f0284fc 9 Motor motorKanan(PB_3, PA_10, PC_4);
SalbiFaza 2:ab055f0284fc 10 Motor motorKiri (PB_9, PA_6, PA_12);
SalbiFaza 2:ab055f0284fc 11 int case_joy;
SalbiFaza 2:ab055f0284fc 12 /****************************************************/
SalbiFaza 2:ab055f0284fc 13 /* Deklarasi Fungsi dan Procedure */
SalbiFaza 2:ab055f0284fc 14 /****************************************************/
SalbiFaza 2:ab055f0284fc 15 int case_joystick()
SalbiFaza 2:ab055f0284fc 16 {
SalbiFaza 2:ab055f0284fc 17 //---------------------------------------------------//
SalbiFaza 2:ab055f0284fc 18 // Gerak Motor Base //
SalbiFaza 2:ab055f0284fc 19 // Case 1 : Pivot kanan //
SalbiFaza 2:ab055f0284fc 20 // Case 2 : Pivot Kiri //
SalbiFaza 2:ab055f0284fc 21 // Case 3 : Maju //
SalbiFaza 2:ab055f0284fc 22 // Case 4 : Kiri //
SalbiFaza 2:ab055f0284fc 23 // Case 5 : Bawah //
SalbiFaza 2:ab055f0284fc 24 // Case 6 : Kanan //
SalbiFaza 2:ab055f0284fc 25 //---------------------------------------------------//
SalbiFaza 2:ab055f0284fc 26
SalbiFaza 2:ab055f0284fc 27 int caseJoystick;
SalbiFaza 2:ab055f0284fc 28 if ((joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
SalbiFaza 2:ab055f0284fc 29 // Pivot Kanan
SalbiFaza 2:ab055f0284fc 30 caseJoystick = 1;
SalbiFaza 2:ab055f0284fc 31 }
SalbiFaza 2:ab055f0284fc 32 else if ((!joystick.R1)&&(joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
SalbiFaza 2:ab055f0284fc 33 // Pivot Kiri
SalbiFaza 2:ab055f0284fc 34 caseJoystick = 2;
SalbiFaza 2:ab055f0284fc 35 }
SalbiFaza 2:ab055f0284fc 36 else if ((!joystick.R1)&&(!joystick.L1)&&(joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
SalbiFaza 2:ab055f0284fc 37 // Pivot Kiri
SalbiFaza 2:ab055f0284fc 38 caseJoystick = 3;
SalbiFaza 2:ab055f0284fc 39 }
SalbiFaza 2:ab055f0284fc 40 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(!joystick.kanan)&&(joystick.kiri)) {
SalbiFaza 2:ab055f0284fc 41 // Pivot Kiri
SalbiFaza 2:ab055f0284fc 42 caseJoystick = 4;
SalbiFaza 2:ab055f0284fc 43 }
SalbiFaza 2:ab055f0284fc 44 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(joystick.bawah)&&(!joystick.kanan)&&(!joystick.kiri)) {
SalbiFaza 2:ab055f0284fc 45 // Pivot Kiri
SalbiFaza 2:ab055f0284fc 46 caseJoystick = 5;
SalbiFaza 2:ab055f0284fc 47 }
SalbiFaza 2:ab055f0284fc 48 else if ((!joystick.R1)&&(!joystick.L1)&&(!joystick.atas)&&(!joystick.bawah)&&(joystick.kanan)&&(!joystick.kiri)) {
SalbiFaza 2:ab055f0284fc 49 // Pivot Kiri
SalbiFaza 2:ab055f0284fc 50 caseJoystick = 6;
SalbiFaza 2:ab055f0284fc 51 }
SalbiFaza 2:ab055f0284fc 52
SalbiFaza 2:ab055f0284fc 53 return(caseJoystick);
SalbiFaza 2:ab055f0284fc 54 }
SalbiFaza 2:ab055f0284fc 55
SalbiFaza 2:ab055f0284fc 56 void aktuator()
SalbiFaza 2:ab055f0284fc 57 {
SalbiFaza 2:ab055f0284fc 58 switch (case_joy) {
SalbiFaza 2:ab055f0284fc 59 case (1):
SalbiFaza 2:ab055f0284fc 60 {
SalbiFaza 2:ab055f0284fc 61 // Pivot Kanan
SalbiFaza 2:ab055f0284fc 62 motorDepan.speed(0.23);
SalbiFaza 2:ab055f0284fc 63 motorKanan.speed(0.17);
SalbiFaza 2:ab055f0284fc 64 motorKiri.speed(0.23);
SalbiFaza 2:ab055f0284fc 65 break;
SalbiFaza 2:ab055f0284fc 66 }
SalbiFaza 2:ab055f0284fc 67 case (2):
SalbiFaza 2:ab055f0284fc 68 {
SalbiFaza 2:ab055f0284fc 69 // Pivot Kiri
SalbiFaza 2:ab055f0284fc 70 motorDepan.speed(-0.23);
SalbiFaza 2:ab055f0284fc 71 motorKanan.speed(-0.17);
SalbiFaza 2:ab055f0284fc 72 motorKiri.speed(-0.23);
SalbiFaza 2:ab055f0284fc 73 break;
SalbiFaza 2:ab055f0284fc 74 }
SalbiFaza 2:ab055f0284fc 75 case (3):
SalbiFaza 2:ab055f0284fc 76 {
SalbiFaza 2:ab055f0284fc 77 // Maju
SalbiFaza 2:ab055f0284fc 78 motorDepan.speed(0);
SalbiFaza 2:ab055f0284fc 79 motorKanan.speed(-0.17);
SalbiFaza 2:ab055f0284fc 80 motorKiri.speed(0.3);
SalbiFaza 2:ab055f0284fc 81 break;
SalbiFaza 2:ab055f0284fc 82 }
SalbiFaza 2:ab055f0284fc 83 case (4):
SalbiFaza 2:ab055f0284fc 84 {
SalbiFaza 2:ab055f0284fc 85 // Kiri
SalbiFaza 2:ab055f0284fc 86 motorDepan.speed(-0.23);
SalbiFaza 2:ab055f0284fc 87 motorKanan.speed(0.17);
SalbiFaza 2:ab055f0284fc 88 motorKiri.speed(0.3);
SalbiFaza 2:ab055f0284fc 89 break;
SalbiFaza 2:ab055f0284fc 90 }
SalbiFaza 2:ab055f0284fc 91 case (5):
SalbiFaza 2:ab055f0284fc 92 {
SalbiFaza 2:ab055f0284fc 93 //Belakang
SalbiFaza 2:ab055f0284fc 94 motorDepan.speed(0);
SalbiFaza 2:ab055f0284fc 95 motorKanan.speed(0.17);
SalbiFaza 2:ab055f0284fc 96 motorKiri.speed(-0.3);
SalbiFaza 2:ab055f0284fc 97 break;
SalbiFaza 2:ab055f0284fc 98 }
SalbiFaza 2:ab055f0284fc 99 case (6):
SalbiFaza 2:ab055f0284fc 100 {
SalbiFaza 2:ab055f0284fc 101 // Kanan
SalbiFaza 2:ab055f0284fc 102 motorDepan.speed(0.23);
SalbiFaza 2:ab055f0284fc 103 motorKanan.speed(-0.17);
SalbiFaza 2:ab055f0284fc 104 motorKiri.speed(-0.3);
SalbiFaza 2:ab055f0284fc 105 break;
SalbiFaza 2:ab055f0284fc 106 }
SalbiFaza 2:ab055f0284fc 107 default :
SalbiFaza 2:ab055f0284fc 108 {
SalbiFaza 2:ab055f0284fc 109 motorDepan.brake(1);
SalbiFaza 2:ab055f0284fc 110 motorKanan.brake(1);
SalbiFaza 2:ab055f0284fc 111 motorKiri.brake(1);
SalbiFaza 2:ab055f0284fc 112 }
SalbiFaza 2:ab055f0284fc 113 }
SalbiFaza 2:ab055f0284fc 114 }
SalbiFaza 2:ab055f0284fc 115 /*********************************************************/
SalbiFaza 2:ab055f0284fc 116 /* Main Function */
SalbiFaza 2:ab055f0284fc 117 /*********************************************************/
SalbiFaza 2:ab055f0284fc 118
SalbiFaza 2:ab055f0284fc 119 int main (void)
SalbiFaza 2:ab055f0284fc 120 {
SalbiFaza 2:ab055f0284fc 121 // Set baud rate - 115200
SalbiFaza 2:ab055f0284fc 122 joystick.setup();
SalbiFaza 2:ab055f0284fc 123 pc.baud(115200);
SalbiFaza 2:ab055f0284fc 124 wait_ms(1000);
SalbiFaza 2:ab055f0284fc 125 // startMillis();
SalbiFaza 2:ab055f0284fc 126 while(1)
SalbiFaza 2:ab055f0284fc 127 {
SalbiFaza 2:ab055f0284fc 128 // Interrupt Serial
SalbiFaza 2:ab055f0284fc 129 joystick.idle();
SalbiFaza 2:ab055f0284fc 130 if(joystick.readable())
SalbiFaza 2:ab055f0284fc 131 {
SalbiFaza 2:ab055f0284fc 132 // Panggil fungsi pembacaan joystik
SalbiFaza 2:ab055f0284fc 133 joystick.baca_data();
SalbiFaza 2:ab055f0284fc 134 // Panggil fungsi pengolahan data joystik
SalbiFaza 2:ab055f0284fc 135 joystick.olah_data();
SalbiFaza 2:ab055f0284fc 136 // Masuk ke case joystick
SalbiFaza 2:ab055f0284fc 137 case_joy = case_joystick();
SalbiFaza 2:ab055f0284fc 138 //pc.printf("%d\n",case_joy);
SalbiFaza 2:ab055f0284fc 139 aktuator();
SalbiFaza 2:ab055f0284fc 140 }
SalbiFaza 2:ab055f0284fc 141 else
SalbiFaza 2:ab055f0284fc 142 {
SalbiFaza 2:ab055f0284fc 143 joystick.idle();
SalbiFaza 2:ab055f0284fc 144 }
SalbiFaza 2:ab055f0284fc 145
SalbiFaza 2:ab055f0284fc 146 // if(millis() - previousMillis5 >= 400)
SalbiFaza 2:ab055f0284fc 147 // {
SalbiFaza 2:ab055f0284fc 148 // display.write(0,((int) rpm2) / 10);
SalbiFaza 2:ab055f0284fc 149 // display.write(1,((int)rpm2) % 10);
SalbiFaza 2:ab055f0284fc 150 // display.write(2, (int)target_rpm2 / 10);
SalbiFaza 2:ab055f0284fc 151 // display.write(3, (int)target_rpm2 % 10);
SalbiFaza 2:ab055f0284fc 152 // display.setColon(true);
SalbiFaza 2:ab055f0284fc 153 //
SalbiFaza 2:ab055f0284fc 154 // previousMillis5 = millis();
SalbiFaza 2:ab055f0284fc 155 // }
SalbiFaza 2:ab055f0284fc 156
SalbiFaza 2:ab055f0284fc 157 }