EL4121 Embedded System / Mbed OS odometry_omni_3roda_v3

PID + multiple point

Dependencies:   Motor PS_PAD TextLCD mbed-os

Fork of cobaLCDJoyMotor_Thread by EL4121 Embedded System

main.cpp@4:cd5de3b14797, 2017-12-12 (annotated)

Committer:
be_bryan
Date:
Tue Dec 12 07:48:20 2017 +0000
Revision:
4:cd5de3b14797
Parent:
3:a03ce2084ceb
Child:
5:0b555929c5b2
llala

Who changed what in which revision?

UserRevisionLine numberNew contents of line
rizqicahyo 1:2bf3dac65b08 1
rizqicahyo 0:837acb06c892 2 #include "mbed.h"
rizqicahyo 0:837acb06c892 3 #include "TextLCD.h"
rizqicahyo 3:a03ce2084ceb 4 #include "PS_PAD.h"
be_bryan 4:cd5de3b14797 5 #include "Motor.h"
be_bryan 4:cd5de3b14797 6 #include "encoderKRAI.h"
rizqicahyo 0:837acb06c892 7
rizqicahyo 3:a03ce2084ceb 8 #include <string>
rizqicahyo 3:a03ce2084ceb 9 using namespace std;
rizqicahyo 3:a03ce2084ceb 10
be_bryan 4:cd5de3b14797 11 Thread thread2(osPriorityNormal, OS_STACK_SIZE, NULL);
be_bryan 4:cd5de3b14797 12 Thread thread1(osPriorityNormal, OS_STACK_SIZE, NULL);
be_bryan 4:cd5de3b14797 13 Thread thread3(osPriorityNormal, OS_STACK_SIZE, NULL);
rizqicahyo 3:a03ce2084ceb 14
be_bryan 4:cd5de3b14797 15 TextLCD lcd(PA_9, PC_3, PC_2, PA_8, PB_0, PC_15, TextLCD::LCD20x4); //rs,e,d4-d7
be_bryan 4:cd5de3b14797 16 encoderKRAI enc1 (PC_14, PC_13, 11, encoderKRAI::X4_ENCODING);
be_bryan 4:cd5de3b14797 17 encoderKRAI enc2 (PC_0, PC_1, 11, encoderKRAI::X4_ENCODING);
be_bryan 4:cd5de3b14797 18 encoderKRAI enc3 (PB_10, PB_3, 11, encoderKRAI::X4_ENCODING);
rizqicahyo 3:a03ce2084ceb 19 PS_PAD ps2(PB_15,PB_14,PB_13, PC_4); //(mosi, miso, sck, ss)
rizqicahyo 3:a03ce2084ceb 20
be_bryan 4:cd5de3b14797 21 Motor motor3(PB_7, PA_14, PA_15); //motor4
be_bryan 4:cd5de3b14797 22 Motor motor2(PA_11, PA_6, PA_5); //motor2
be_bryan 4:cd5de3b14797 23 //Motor motor1(PA_10, PB_5, PB_4); //motor_griper
be_bryan 4:cd5de3b14797 24 Motor motor1(PB_6, PA_7, PB_12); //motor 3
be_bryan 4:cd5de3b14797 25 string a;
be_bryan 4:cd5de3b14797 26
be_bryan 4:cd5de3b14797 27 void dataJoystick();
be_bryan 4:cd5de3b14797 28 void lcdPrint();
be_bryan 4:cd5de3b14797 29 void motorP();
rizqicahyo 0:837acb06c892 30
rizqicahyo 0:837acb06c892 31 Serial pc(USBTX,USBRX);
rizqicahyo 0:837acb06c892 32
rizqicahyo 0:837acb06c892 33 int main()
rizqicahyo 0:837acb06c892 34 {
rizqicahyo 0:837acb06c892 35 pc.baud(115200);
rizqicahyo 3:a03ce2084ceb 36 ps2.init();
be_bryan 4:cd5de3b14797 37 thread1.start(dataJoystick);
be_bryan 4:cd5de3b14797 38 thread2.start(lcdPrint);
be_bryan 4:cd5de3b14797 39 thread3.start(motorP);
rizqicahyo 0:837acb06c892 40 while (1)
rizqicahyo 0:837acb06c892 41 {
rizqicahyo 3:a03ce2084ceb 42 // baca input
be_bryan 4:cd5de3b14797 43 /*
rizqicahyo 3:a03ce2084ceb 44 ps2.poll();
rizqicahyo 3:a03ce2084ceb 45 if(ps2.read(PS_PAD::PAD_X)==1) a = "silang";
rizqicahyo 3:a03ce2084ceb 46 else if(ps2.read(PS_PAD::PAD_CIRCLE)==1) a = "lingkaran";
rizqicahyo 3:a03ce2084ceb 47 else if(ps2.read(PS_PAD::PAD_TRIANGLE)==1) a = "segitiga";
rizqicahyo 3:a03ce2084ceb 48 else if(ps2.read(PS_PAD::PAD_SQUARE)==1) a = "kotak";
rizqicahyo 3:a03ce2084ceb 49 else a = "NULL";
be_bryan 4:cd5de3b14797 50 */
be_bryan 4:cd5de3b14797 51 /*
rizqicahyo 3:a03ce2084ceb 52 //tampilkan LCD
rizqicahyo 3:a03ce2084ceb 53 lcd.locate(0,0);
rizqicahyo 3:a03ce2084ceb 54 lcd.printf("input joystik :");
rizqicahyo 3:a03ce2084ceb 55 lcd.locate(0,1);
rizqicahyo 3:a03ce2084ceb 56 lcd.printf("%s",a);
rizqicahyo 0:837acb06c892 57
rizqicahyo 0:837acb06c892 58 wait_ms(10);
rizqicahyo 0:837acb06c892 59 lcd.cls();
be_bryan 4:cd5de3b14797 60 */
be_bryan 4:cd5de3b14797 61 }
be_bryan 4:cd5de3b14797 62 }
be_bryan 4:cd5de3b14797 63
be_bryan 4:cd5de3b14797 64 void dataJoystick(){
be_bryan 4:cd5de3b14797 65 while(true){
be_bryan 4:cd5de3b14797 66 ps2.poll();
be_bryan 4:cd5de3b14797 67 if(ps2.read(PS_PAD::PAD_X)==1) a = "silang";
be_bryan 4:cd5de3b14797 68 else if(ps2.read(PS_PAD::PAD_CIRCLE)==1) a = "lingkaran";
be_bryan 4:cd5de3b14797 69 else if(ps2.read(PS_PAD::PAD_TRIANGLE)==1) a = "segitiga";
be_bryan 4:cd5de3b14797 70 else if(ps2.read(PS_PAD::PAD_SQUARE)==1) a = "kotak";
be_bryan 4:cd5de3b14797 71 else a = "NULL";
rizqicahyo 0:837acb06c892 72 }
rizqicahyo 0:837acb06c892 73 }
be_bryan 4:cd5de3b14797 74
be_bryan 4:cd5de3b14797 75 void lcdPrint(){
be_bryan 4:cd5de3b14797 76 while (true){
be_bryan 4:cd5de3b14797 77 lcd.cls();
be_bryan 4:cd5de3b14797 78 int pulse1 = enc1.getPulses();
be_bryan 4:cd5de3b14797 79 int pulse2 = enc2.getPulses();
be_bryan 4:cd5de3b14797 80 int pulse3 = enc3.getPulses();
be_bryan 4:cd5de3b14797 81 lcd.locate(0,0);
be_bryan 4:cd5de3b14797 82 lcd.printf("input : %s", a);
be_bryan 4:cd5de3b14797 83 lcd.locate(0,1);
be_bryan 4:cd5de3b14797 84 lcd.printf("enc1 = %d", pulse1);
be_bryan 4:cd5de3b14797 85 lcd.locate(0,2);
be_bryan 4:cd5de3b14797 86 lcd.printf("enc2 = %d", pulse2);
be_bryan 4:cd5de3b14797 87 lcd.locate(0,3);
be_bryan 4:cd5de3b14797 88 lcd.printf("enc3 = %d", pulse3);
be_bryan 4:cd5de3b14797 89 Thread::wait(20);
be_bryan 4:cd5de3b14797 90 }
be_bryan 4:cd5de3b14797 91 }
be_bryan 4:cd5de3b14797 92
be_bryan 4:cd5de3b14797 93 void motorP() {
be_bryan 4:cd5de3b14797 94 while(true){
be_bryan 4:cd5de3b14797 95 motor1.speed(0.5);
be_bryan 4:cd5de3b14797 96 motor2.speed(0.5);
be_bryan 4:cd5de3b14797 97 motor3.speed(0.5);
be_bryan 4:cd5de3b14797 98 Thread::wait(2000);
be_bryan 4:cd5de3b14797 99 motor1.speed(-0.5);
be_bryan 4:cd5de3b14797 100 motor2.speed(-0.5);
be_bryan 4:cd5de3b14797 101 motor3.speed(-0.5);
be_bryan 4:cd5de3b14797 102 Thread::wait(2000);
be_bryan 4:cd5de3b14797 103 }
be_bryan 4:cd5de3b14797 104 }