KATAPANG
/
pangan
baru
main.cpp
- Committer:
- aldo240498
- Date:
- 2019-05-14
- Revision:
- 0:aa2dd4e13c71
File content as of revision 0:aa2dd4e13c71:
#include"mbed.h" #include "stdio.h" #include "Serial.h" #include "CMPS03.h" /////////////////////motor/////////////////////// DigitalOut dir3(PB_9); DigitalOut dir4(PB_8); DigitalOut dir5(PC_5); DigitalOut dir6(PC_8); DigitalOut dir1(PA_12); DigitalOut dir2(PB_12); DigitalOut dir7(PB_14); DigitalOut dir8(PB_15); PwmOut pwm4(PB_1); PwmOut pwm3(PC_9); PwmOut pwm2(PC_6); PwmOut pwm1(PA_11); //////////////////////////////////////////////// float roda1, roda2, roda3, roda4; float pw1,pw2,pw3, pw4; float abs_pwm1, abs_pwm2, abs_pwm3, abs_pwm4; double dx, dy, dz; //////////////////////inverse/////////////////////// void atur(float x, float y, float z) { roda1 = x - z + y; roda2 = y - z + x; roda3 = y + z - x; roda4 = x + z - y; if (roda1 < 0) //kiri atas { abs_pwm1 = abs(roda1); pw1=(float) abs_pwm1/255; dir1 = 1; dir2 = 0; } else { abs_pwm1 = roda1; pw1=(float) abs_pwm1/255; dir1 = 0; dir2 = 1; } if (roda2 < 0) // kanan atas { abs_pwm2 = abs(roda2); pw2=(float) abs_pwm2/255; dir3 = 0; dir4 = 1; } else { abs_pwm2 = roda2; pw2=(float) abs_pwm2/255; dir3 = 1; dir4 = 0; } if (roda3 < 0) // kanan bawah { abs_pwm3 = abs(roda3); pw3=(float) abs_pwm3/255; dir5 = 1; dir6 = 0; } else { abs_pwm3 = roda3; pw3=(float) abs_pwm3/255; dir5 = 0; dir6 = 1; } if (roda4 < 0) // kiri bawah { abs_pwm4 = abs(roda4); pw4= ((float) abs_pwm4/255); dir7 = 1; dir8 = 0; } else { abs_pwm4 = roda4; pw4=((float) abs_pwm4/255); dir7 = 0; dir8 = 1; } pwm1.write(pw1); pwm2.write(pw2); pwm3.write(pw3); pwm4.write(pw4); } //////////////////////////////////////////////// Serial pc(USBTX, USBRX); CMPS03 compass(PB_4,PA_8 , CMPS03_DEFAULT_I2C_ADDRESS); Serial device(PB_6, PB_7); PinName pin_SERVO0 = PB_0; PinName pin_SERVO1 = PB_3; PinName pin_SERVO2 = PA_0; PinName pin_SERVO3 = PA_10; PinName pin_SERVO4 = PA_5; PinName pin_SERVO5 = PA_7; PinName pin_SERVO6 = PA_1; PinName pin_SERVO7 = PB_10; PinName pin_SERVO8 = PB_5; DigitalOut relay1(PA_13); PwmOut Servo0(pin_SERVO0); PwmOut Servo1(pin_SERVO1); PwmOut Servo2(pin_SERVO2); PwmOut Servo3(pin_SERVO3); PwmOut Servo4(pin_SERVO4); PwmOut Servo5(pin_SERVO5); PwmOut Servo6(pin_SERVO6); PwmOut Servo7(pin_SERVO7); PwmOut Servo8(pin_SERVO8); void Servo_angle(int16_t angle) { int16_t Angle = 600 + (angle * 10); Servo0.pulsewidth_us(Angle); Servo1.pulsewidth_us(Angle); Servo2.pulsewidth_us(Angle); Servo3.pulsewidth_us(Angle); Servo4.pulsewidth_us(Angle); Servo5.pulsewidth_us(Angle); Servo6.pulsewidth_us(Angle); Servo7.pulsewidth_us(Angle); Servo8.pulsewidth_us(Angle); } int inputdata; int data; int data2; double posisi; int main() { device.baud(38400); pc.baud(57600); Servo0.period_ms(20); Servo1.period_ms(20); Servo2.period_ms(20); Servo3.period_ms(20); Servo4.period_ms(20); Servo5.period_ms(20); Servo6.period_ms(20); Servo7.period_ms(20); Servo8.period_ms(20); while(1) { if(device.readable()>0) { inputdata = device.getc(); data=inputdata; pc.putc(inputdata); { //penanaman if(data == 'i') //tutup { // 180 ~ 0 for(int i=180; i>=0; i--) { int16_t Angle = 600 + (i * 10); Servo2.pulsewidth_us(Angle); Servo3.pulsewidth_us(Angle); Servo4.pulsewidth_us(Angle); Servo7.pulsewidth_us(Angle); } } if(data == 'h') //buka { // 180 ~ 0 for(int i=0; i<=110; i++) { int16_t Angle = 600 + (i * 10); Servo2.pulsewidth_us(Angle); Servo3.pulsewidth_us(Angle); Servo4.pulsewidth_us(Angle); Servo7.pulsewidth_us(Angle); } } if(data == 'g') //arm naik { // 180 ~ 0 for(int i=180; i>=100; i--) { int16_t Angle = 600 + (i * 10); Servo0.pulsewidth_us(Angle); Servo1.pulsewidth_us(Angle); Servo5.pulsewidth_us(Angle); Servo6.pulsewidth_us(Angle); Servo8.pulsewidth_us(Angle); } } if(data == 'f') //arm turun { // 0 ~ 180 for(int i=100; i<=180; i++) { int16_t Angle = 600 + (i * 10); Servo0.pulsewidth_us(Angle); Servo1.pulsewidth_us(Angle); Servo5.pulsewidth_us(Angle); Servo6.pulsewidth_us(Angle); Servo8.pulsewidth_us(Angle); } } // penyiangan if(data == 'b') //arm turun { // 0 ~ 180 for(int i=100; i<=180; i++) { int16_t Angle = 600 + (i * 10); Servo0.pulsewidth_us(Angle); //Servo1.pulsewidth_us(Angle); Servo5.pulsewidth_us(Angle); } } if(data == 'c') //arm naik { // 180 ~ 0 for(int i=180; i>=100; i--) { int16_t Angle = 600 + (i * 10); Servo0.pulsewidth_us(Angle); //Servo1.pulsewidth_us(Angle); Servo5.pulsewidth_us(Angle); } } if(data == 'e') //tutup { // 180 ~ 0 for(int i=180; i>=0; i--) { int16_t Angle = 600 + (i * 10); Servo2.pulsewidth_us(Angle); //Servo3.pulsewidth_us(Angle); Servo4.pulsewidth_us(Angle); } } if(data == 'd') //buka { // 180 ~ 0 for(int i=0; i<=110; i++) { int16_t Angle = 600 + (i * 10); Servo2.pulsewidth_us(Angle); //Servo3.pulsewidth_us(Angle); Servo4.pulsewidth_us(Angle); } } switch(data){ case'2': atur(50,0,0); //maju break; case'3': atur(-50,0,0); //mundur break; case'7': atur(0,-50,0); //kanan break; case'1': atur(0,50,0); //kiri break; case'5': atur(0,0,50); //muter kanan break; case'8': atur(0,0,-50); //muter kiri break; case'9': { for(double posisi=0; posisi<=329.700012; posisi++) { posisi=compass.readBearing()/10.0; printf("Bearing is: %f\n", posisi); atur(0,0,100); if(posisi==329.700012); } atur(0,0,0); default: dir1 = 1; dir2 = 1; dir3 = 1; dir4 = 1; dir5 = 1; dir6 = 1; dir7 = 1; dir8 = 1; ; } /* if(data == '3') //mundur { atur(100,0,0); } if(data == '7') //kanan { atur(0,100,0); } if(data == '1') //kiri { atur(0,-100,0); } if(data == '4') //armkanan { atur(0,0,150); } if(data == '8') //armkanan { atur(0,0,-150); } if(data == 'r') //armkanan { atur(0,0,0); } */ // untuk mode pemotong menggunakan relay if(data == 'a'){ relay1=1; } if(data == 'z') { relay1=0; } } }}}}