KRTMI 2019 / Mbed 2 deprecated KRTMI_v1

code asli

Dependencies:   mbed Servo encoderKRTMI Motornew Lengan3 millis

main.cpp@0:96a0fbdff740, 2019-02-18 (annotated)

Committer:
bernardusrendy
Date:
Mon Feb 18 12:54:07 2019 +0000
Revision:
0:96a0fbdff740
test

Who changed what in which revision?

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