KRTMI 2019 / Mbed 2 deprecated KRTMI_v1

code asli

Dependencies:   mbed Servo encoderKRTMI Motornew Lengan3 millis

main.cpp@3:6d469c371f49, 2019-05-19 (annotated)

Committer:
harrymunli
Date:
Sun May 19 15:59:31 2019 +0000
Revision:
3:6d469c371f49
Parent:
2:ab055f0284fc 
code KRTMI v3

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