KATAPANG
/
pangan
baru
main.cpp@0:aa2dd4e13c71, 2019-05-14 (annotated)
- Committer:
- aldo240498
- Date:
- Tue May 14 05:17:26 2019 +0000
- Revision:
- 0:aa2dd4e13c71
v1
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
aldo240498 | 0:aa2dd4e13c71 | 1 | #include"mbed.h" |
aldo240498 | 0:aa2dd4e13c71 | 2 | #include "stdio.h" |
aldo240498 | 0:aa2dd4e13c71 | 3 | #include "Serial.h" |
aldo240498 | 0:aa2dd4e13c71 | 4 | #include "CMPS03.h" |
aldo240498 | 0:aa2dd4e13c71 | 5 | |
aldo240498 | 0:aa2dd4e13c71 | 6 | /////////////////////motor/////////////////////// |
aldo240498 | 0:aa2dd4e13c71 | 7 | DigitalOut dir3(PB_9); |
aldo240498 | 0:aa2dd4e13c71 | 8 | DigitalOut dir4(PB_8); |
aldo240498 | 0:aa2dd4e13c71 | 9 | DigitalOut dir5(PC_5); |
aldo240498 | 0:aa2dd4e13c71 | 10 | DigitalOut dir6(PC_8); |
aldo240498 | 0:aa2dd4e13c71 | 11 | DigitalOut dir1(PA_12); |
aldo240498 | 0:aa2dd4e13c71 | 12 | DigitalOut dir2(PB_12); |
aldo240498 | 0:aa2dd4e13c71 | 13 | DigitalOut dir7(PB_14); |
aldo240498 | 0:aa2dd4e13c71 | 14 | DigitalOut dir8(PB_15); |
aldo240498 | 0:aa2dd4e13c71 | 15 | PwmOut pwm4(PB_1); |
aldo240498 | 0:aa2dd4e13c71 | 16 | PwmOut pwm3(PC_9); |
aldo240498 | 0:aa2dd4e13c71 | 17 | PwmOut pwm2(PC_6); |
aldo240498 | 0:aa2dd4e13c71 | 18 | PwmOut pwm1(PA_11); |
aldo240498 | 0:aa2dd4e13c71 | 19 | |
aldo240498 | 0:aa2dd4e13c71 | 20 | //////////////////////////////////////////////// |
aldo240498 | 0:aa2dd4e13c71 | 21 | |
aldo240498 | 0:aa2dd4e13c71 | 22 | |
aldo240498 | 0:aa2dd4e13c71 | 23 | |
aldo240498 | 0:aa2dd4e13c71 | 24 | |
aldo240498 | 0:aa2dd4e13c71 | 25 | |
aldo240498 | 0:aa2dd4e13c71 | 26 | float roda1, roda2, roda3, roda4; |
aldo240498 | 0:aa2dd4e13c71 | 27 | float pw1,pw2,pw3, pw4; |
aldo240498 | 0:aa2dd4e13c71 | 28 | float abs_pwm1, abs_pwm2, abs_pwm3, abs_pwm4; |
aldo240498 | 0:aa2dd4e13c71 | 29 | double dx, dy, dz; |
aldo240498 | 0:aa2dd4e13c71 | 30 | |
aldo240498 | 0:aa2dd4e13c71 | 31 | //////////////////////inverse/////////////////////// |
aldo240498 | 0:aa2dd4e13c71 | 32 | |
aldo240498 | 0:aa2dd4e13c71 | 33 | |
aldo240498 | 0:aa2dd4e13c71 | 34 | void atur(float x, float y, float z) |
aldo240498 | 0:aa2dd4e13c71 | 35 | { |
aldo240498 | 0:aa2dd4e13c71 | 36 | |
aldo240498 | 0:aa2dd4e13c71 | 37 | roda1 = x - z + y; |
aldo240498 | 0:aa2dd4e13c71 | 38 | roda2 = y - z + x; |
aldo240498 | 0:aa2dd4e13c71 | 39 | roda3 = y + z - x; |
aldo240498 | 0:aa2dd4e13c71 | 40 | roda4 = x + z - y; |
aldo240498 | 0:aa2dd4e13c71 | 41 | |
aldo240498 | 0:aa2dd4e13c71 | 42 | if (roda1 < 0) //kiri atas |
aldo240498 | 0:aa2dd4e13c71 | 43 | { |
aldo240498 | 0:aa2dd4e13c71 | 44 | abs_pwm1 = abs(roda1); |
aldo240498 | 0:aa2dd4e13c71 | 45 | pw1=(float) abs_pwm1/255; |
aldo240498 | 0:aa2dd4e13c71 | 46 | dir1 = 1; |
aldo240498 | 0:aa2dd4e13c71 | 47 | dir2 = 0; |
aldo240498 | 0:aa2dd4e13c71 | 48 | } |
aldo240498 | 0:aa2dd4e13c71 | 49 | else |
aldo240498 | 0:aa2dd4e13c71 | 50 | { |
aldo240498 | 0:aa2dd4e13c71 | 51 | abs_pwm1 = roda1; |
aldo240498 | 0:aa2dd4e13c71 | 52 | pw1=(float) abs_pwm1/255; |
aldo240498 | 0:aa2dd4e13c71 | 53 | dir1 = 0; |
aldo240498 | 0:aa2dd4e13c71 | 54 | dir2 = 1; |
aldo240498 | 0:aa2dd4e13c71 | 55 | } |
aldo240498 | 0:aa2dd4e13c71 | 56 | if (roda2 < 0) // kanan atas |
aldo240498 | 0:aa2dd4e13c71 | 57 | { |
aldo240498 | 0:aa2dd4e13c71 | 58 | abs_pwm2 = abs(roda2); |
aldo240498 | 0:aa2dd4e13c71 | 59 | pw2=(float) abs_pwm2/255; |
aldo240498 | 0:aa2dd4e13c71 | 60 | dir3 = 0; |
aldo240498 | 0:aa2dd4e13c71 | 61 | dir4 = 1; |
aldo240498 | 0:aa2dd4e13c71 | 62 | } |
aldo240498 | 0:aa2dd4e13c71 | 63 | else |
aldo240498 | 0:aa2dd4e13c71 | 64 | { |
aldo240498 | 0:aa2dd4e13c71 | 65 | abs_pwm2 = roda2; |
aldo240498 | 0:aa2dd4e13c71 | 66 | pw2=(float) abs_pwm2/255; |
aldo240498 | 0:aa2dd4e13c71 | 67 | dir3 = 1; |
aldo240498 | 0:aa2dd4e13c71 | 68 | dir4 = 0; |
aldo240498 | 0:aa2dd4e13c71 | 69 | } |
aldo240498 | 0:aa2dd4e13c71 | 70 | if (roda3 < 0) // kanan bawah |
aldo240498 | 0:aa2dd4e13c71 | 71 | { |
aldo240498 | 0:aa2dd4e13c71 | 72 | abs_pwm3 = abs(roda3); |
aldo240498 | 0:aa2dd4e13c71 | 73 | pw3=(float) abs_pwm3/255; |
aldo240498 | 0:aa2dd4e13c71 | 74 | dir5 = 1; |
aldo240498 | 0:aa2dd4e13c71 | 75 | dir6 = 0; |
aldo240498 | 0:aa2dd4e13c71 | 76 | } |
aldo240498 | 0:aa2dd4e13c71 | 77 | else |
aldo240498 | 0:aa2dd4e13c71 | 78 | { |
aldo240498 | 0:aa2dd4e13c71 | 79 | abs_pwm3 = roda3; |
aldo240498 | 0:aa2dd4e13c71 | 80 | pw3=(float) abs_pwm3/255; |
aldo240498 | 0:aa2dd4e13c71 | 81 | dir5 = 0; |
aldo240498 | 0:aa2dd4e13c71 | 82 | dir6 = 1; |
aldo240498 | 0:aa2dd4e13c71 | 83 | } |
aldo240498 | 0:aa2dd4e13c71 | 84 | if (roda4 < 0) // kiri bawah |
aldo240498 | 0:aa2dd4e13c71 | 85 | { |
aldo240498 | 0:aa2dd4e13c71 | 86 | abs_pwm4 = abs(roda4); |
aldo240498 | 0:aa2dd4e13c71 | 87 | pw4= ((float) abs_pwm4/255); |
aldo240498 | 0:aa2dd4e13c71 | 88 | dir7 = 1; |
aldo240498 | 0:aa2dd4e13c71 | 89 | dir8 = 0; |
aldo240498 | 0:aa2dd4e13c71 | 90 | } |
aldo240498 | 0:aa2dd4e13c71 | 91 | else |
aldo240498 | 0:aa2dd4e13c71 | 92 | { |
aldo240498 | 0:aa2dd4e13c71 | 93 | abs_pwm4 = roda4; |
aldo240498 | 0:aa2dd4e13c71 | 94 | pw4=((float) abs_pwm4/255); |
aldo240498 | 0:aa2dd4e13c71 | 95 | dir7 = 0; |
aldo240498 | 0:aa2dd4e13c71 | 96 | dir8 = 1; |
aldo240498 | 0:aa2dd4e13c71 | 97 | } |
aldo240498 | 0:aa2dd4e13c71 | 98 | pwm1.write(pw1); |
aldo240498 | 0:aa2dd4e13c71 | 99 | pwm2.write(pw2); |
aldo240498 | 0:aa2dd4e13c71 | 100 | pwm3.write(pw3); |
aldo240498 | 0:aa2dd4e13c71 | 101 | pwm4.write(pw4); |
aldo240498 | 0:aa2dd4e13c71 | 102 | } |
aldo240498 | 0:aa2dd4e13c71 | 103 | |
aldo240498 | 0:aa2dd4e13c71 | 104 | //////////////////////////////////////////////// |
aldo240498 | 0:aa2dd4e13c71 | 105 | |
aldo240498 | 0:aa2dd4e13c71 | 106 | |
aldo240498 | 0:aa2dd4e13c71 | 107 | Serial pc(USBTX, USBRX); |
aldo240498 | 0:aa2dd4e13c71 | 108 | CMPS03 compass(PB_4,PA_8 , CMPS03_DEFAULT_I2C_ADDRESS); |
aldo240498 | 0:aa2dd4e13c71 | 109 | Serial device(PB_6, PB_7); |
aldo240498 | 0:aa2dd4e13c71 | 110 | PinName pin_SERVO0 = PB_0; |
aldo240498 | 0:aa2dd4e13c71 | 111 | PinName pin_SERVO1 = PB_3; |
aldo240498 | 0:aa2dd4e13c71 | 112 | PinName pin_SERVO2 = PA_0; |
aldo240498 | 0:aa2dd4e13c71 | 113 | PinName pin_SERVO3 = PA_10; |
aldo240498 | 0:aa2dd4e13c71 | 114 | PinName pin_SERVO4 = PA_5; |
aldo240498 | 0:aa2dd4e13c71 | 115 | PinName pin_SERVO5 = PA_7; |
aldo240498 | 0:aa2dd4e13c71 | 116 | PinName pin_SERVO6 = PA_1; |
aldo240498 | 0:aa2dd4e13c71 | 117 | PinName pin_SERVO7 = PB_10; |
aldo240498 | 0:aa2dd4e13c71 | 118 | PinName pin_SERVO8 = PB_5; |
aldo240498 | 0:aa2dd4e13c71 | 119 | |
aldo240498 | 0:aa2dd4e13c71 | 120 | |
aldo240498 | 0:aa2dd4e13c71 | 121 | |
aldo240498 | 0:aa2dd4e13c71 | 122 | |
aldo240498 | 0:aa2dd4e13c71 | 123 | |
aldo240498 | 0:aa2dd4e13c71 | 124 | DigitalOut relay1(PA_13); |
aldo240498 | 0:aa2dd4e13c71 | 125 | PwmOut Servo0(pin_SERVO0); |
aldo240498 | 0:aa2dd4e13c71 | 126 | PwmOut Servo1(pin_SERVO1); |
aldo240498 | 0:aa2dd4e13c71 | 127 | PwmOut Servo2(pin_SERVO2); |
aldo240498 | 0:aa2dd4e13c71 | 128 | PwmOut Servo3(pin_SERVO3); |
aldo240498 | 0:aa2dd4e13c71 | 129 | PwmOut Servo4(pin_SERVO4); |
aldo240498 | 0:aa2dd4e13c71 | 130 | PwmOut Servo5(pin_SERVO5); |
aldo240498 | 0:aa2dd4e13c71 | 131 | PwmOut Servo6(pin_SERVO6); |
aldo240498 | 0:aa2dd4e13c71 | 132 | PwmOut Servo7(pin_SERVO7); |
aldo240498 | 0:aa2dd4e13c71 | 133 | PwmOut Servo8(pin_SERVO8); |
aldo240498 | 0:aa2dd4e13c71 | 134 | |
aldo240498 | 0:aa2dd4e13c71 | 135 | void Servo_angle(int16_t angle) |
aldo240498 | 0:aa2dd4e13c71 | 136 | { |
aldo240498 | 0:aa2dd4e13c71 | 137 | int16_t Angle = 600 + (angle * 10); |
aldo240498 | 0:aa2dd4e13c71 | 138 | Servo0.pulsewidth_us(Angle); |
aldo240498 | 0:aa2dd4e13c71 | 139 | Servo1.pulsewidth_us(Angle); |
aldo240498 | 0:aa2dd4e13c71 | 140 | Servo2.pulsewidth_us(Angle); |
aldo240498 | 0:aa2dd4e13c71 | 141 | Servo3.pulsewidth_us(Angle); |
aldo240498 | 0:aa2dd4e13c71 | 142 | Servo4.pulsewidth_us(Angle); |
aldo240498 | 0:aa2dd4e13c71 | 143 | Servo5.pulsewidth_us(Angle); |
aldo240498 | 0:aa2dd4e13c71 | 144 | Servo6.pulsewidth_us(Angle); |
aldo240498 | 0:aa2dd4e13c71 | 145 | Servo7.pulsewidth_us(Angle); |
aldo240498 | 0:aa2dd4e13c71 | 146 | Servo8.pulsewidth_us(Angle); |
aldo240498 | 0:aa2dd4e13c71 | 147 | |
aldo240498 | 0:aa2dd4e13c71 | 148 | } |
aldo240498 | 0:aa2dd4e13c71 | 149 | |
aldo240498 | 0:aa2dd4e13c71 | 150 | |
aldo240498 | 0:aa2dd4e13c71 | 151 | |
aldo240498 | 0:aa2dd4e13c71 | 152 | int inputdata; |
aldo240498 | 0:aa2dd4e13c71 | 153 | int data; |
aldo240498 | 0:aa2dd4e13c71 | 154 | int data2; |
aldo240498 | 0:aa2dd4e13c71 | 155 | double posisi; |
aldo240498 | 0:aa2dd4e13c71 | 156 | int main() |
aldo240498 | 0:aa2dd4e13c71 | 157 | { |
aldo240498 | 0:aa2dd4e13c71 | 158 | device.baud(38400); |
aldo240498 | 0:aa2dd4e13c71 | 159 | pc.baud(57600); |
aldo240498 | 0:aa2dd4e13c71 | 160 | Servo0.period_ms(20); |
aldo240498 | 0:aa2dd4e13c71 | 161 | Servo1.period_ms(20); |
aldo240498 | 0:aa2dd4e13c71 | 162 | Servo2.period_ms(20); |
aldo240498 | 0:aa2dd4e13c71 | 163 | Servo3.period_ms(20); |
aldo240498 | 0:aa2dd4e13c71 | 164 | Servo4.period_ms(20); |
aldo240498 | 0:aa2dd4e13c71 | 165 | Servo5.period_ms(20); |
aldo240498 | 0:aa2dd4e13c71 | 166 | Servo6.period_ms(20); |
aldo240498 | 0:aa2dd4e13c71 | 167 | Servo7.period_ms(20); |
aldo240498 | 0:aa2dd4e13c71 | 168 | Servo8.period_ms(20); |
aldo240498 | 0:aa2dd4e13c71 | 169 | |
aldo240498 | 0:aa2dd4e13c71 | 170 | while(1) { |
aldo240498 | 0:aa2dd4e13c71 | 171 | |
aldo240498 | 0:aa2dd4e13c71 | 172 | |
aldo240498 | 0:aa2dd4e13c71 | 173 | |
aldo240498 | 0:aa2dd4e13c71 | 174 | if(device.readable()>0) |
aldo240498 | 0:aa2dd4e13c71 | 175 | { |
aldo240498 | 0:aa2dd4e13c71 | 176 | inputdata = device.getc(); |
aldo240498 | 0:aa2dd4e13c71 | 177 | data=inputdata; |
aldo240498 | 0:aa2dd4e13c71 | 178 | pc.putc(inputdata); |
aldo240498 | 0:aa2dd4e13c71 | 179 | { |
aldo240498 | 0:aa2dd4e13c71 | 180 | |
aldo240498 | 0:aa2dd4e13c71 | 181 | |
aldo240498 | 0:aa2dd4e13c71 | 182 | |
aldo240498 | 0:aa2dd4e13c71 | 183 | |
aldo240498 | 0:aa2dd4e13c71 | 184 | //penanaman |
aldo240498 | 0:aa2dd4e13c71 | 185 | if(data == 'i') //tutup |
aldo240498 | 0:aa2dd4e13c71 | 186 | { |
aldo240498 | 0:aa2dd4e13c71 | 187 | |
aldo240498 | 0:aa2dd4e13c71 | 188 | // 180 ~ 0 |
aldo240498 | 0:aa2dd4e13c71 | 189 | for(int i=180; i>=0; i--) |
aldo240498 | 0:aa2dd4e13c71 | 190 | { |
aldo240498 | 0:aa2dd4e13c71 | 191 | int16_t Angle = 600 + (i * 10); |
aldo240498 | 0:aa2dd4e13c71 | 192 | Servo2.pulsewidth_us(Angle); |
aldo240498 | 0:aa2dd4e13c71 | 193 | Servo3.pulsewidth_us(Angle); |
aldo240498 | 0:aa2dd4e13c71 | 194 | Servo4.pulsewidth_us(Angle); |
aldo240498 | 0:aa2dd4e13c71 | 195 | Servo7.pulsewidth_us(Angle); |
aldo240498 | 0:aa2dd4e13c71 | 196 | |
aldo240498 | 0:aa2dd4e13c71 | 197 | } |
aldo240498 | 0:aa2dd4e13c71 | 198 | } |
aldo240498 | 0:aa2dd4e13c71 | 199 | if(data == 'h') //buka |
aldo240498 | 0:aa2dd4e13c71 | 200 | { |
aldo240498 | 0:aa2dd4e13c71 | 201 | |
aldo240498 | 0:aa2dd4e13c71 | 202 | // 180 ~ 0 |
aldo240498 | 0:aa2dd4e13c71 | 203 | for(int i=0; i<=110; i++) |
aldo240498 | 0:aa2dd4e13c71 | 204 | { |
aldo240498 | 0:aa2dd4e13c71 | 205 | int16_t Angle = 600 + (i * 10); |
aldo240498 | 0:aa2dd4e13c71 | 206 | Servo2.pulsewidth_us(Angle); |
aldo240498 | 0:aa2dd4e13c71 | 207 | Servo3.pulsewidth_us(Angle); |
aldo240498 | 0:aa2dd4e13c71 | 208 | Servo4.pulsewidth_us(Angle); |
aldo240498 | 0:aa2dd4e13c71 | 209 | Servo7.pulsewidth_us(Angle); |
aldo240498 | 0:aa2dd4e13c71 | 210 | |
aldo240498 | 0:aa2dd4e13c71 | 211 | } |
aldo240498 | 0:aa2dd4e13c71 | 212 | } |
aldo240498 | 0:aa2dd4e13c71 | 213 | |
aldo240498 | 0:aa2dd4e13c71 | 214 | |
aldo240498 | 0:aa2dd4e13c71 | 215 | if(data == 'g') //arm naik |
aldo240498 | 0:aa2dd4e13c71 | 216 | { |
aldo240498 | 0:aa2dd4e13c71 | 217 | |
aldo240498 | 0:aa2dd4e13c71 | 218 | // 180 ~ 0 |
aldo240498 | 0:aa2dd4e13c71 | 219 | for(int i=180; i>=100; i--) |
aldo240498 | 0:aa2dd4e13c71 | 220 | { |
aldo240498 | 0:aa2dd4e13c71 | 221 | int16_t Angle = 600 + (i * 10); |
aldo240498 | 0:aa2dd4e13c71 | 222 | Servo0.pulsewidth_us(Angle); |
aldo240498 | 0:aa2dd4e13c71 | 223 | Servo1.pulsewidth_us(Angle); |
aldo240498 | 0:aa2dd4e13c71 | 224 | Servo5.pulsewidth_us(Angle); |
aldo240498 | 0:aa2dd4e13c71 | 225 | Servo6.pulsewidth_us(Angle); |
aldo240498 | 0:aa2dd4e13c71 | 226 | Servo8.pulsewidth_us(Angle); |
aldo240498 | 0:aa2dd4e13c71 | 227 | |
aldo240498 | 0:aa2dd4e13c71 | 228 | } |
aldo240498 | 0:aa2dd4e13c71 | 229 | } |
aldo240498 | 0:aa2dd4e13c71 | 230 | |
aldo240498 | 0:aa2dd4e13c71 | 231 | if(data == 'f') //arm turun |
aldo240498 | 0:aa2dd4e13c71 | 232 | { |
aldo240498 | 0:aa2dd4e13c71 | 233 | // 0 ~ 180 |
aldo240498 | 0:aa2dd4e13c71 | 234 | for(int i=100; i<=180; i++) |
aldo240498 | 0:aa2dd4e13c71 | 235 | { |
aldo240498 | 0:aa2dd4e13c71 | 236 | int16_t Angle = 600 + (i * 10); |
aldo240498 | 0:aa2dd4e13c71 | 237 | Servo0.pulsewidth_us(Angle); |
aldo240498 | 0:aa2dd4e13c71 | 238 | Servo1.pulsewidth_us(Angle); |
aldo240498 | 0:aa2dd4e13c71 | 239 | Servo5.pulsewidth_us(Angle); |
aldo240498 | 0:aa2dd4e13c71 | 240 | Servo6.pulsewidth_us(Angle); |
aldo240498 | 0:aa2dd4e13c71 | 241 | Servo8.pulsewidth_us(Angle); |
aldo240498 | 0:aa2dd4e13c71 | 242 | |
aldo240498 | 0:aa2dd4e13c71 | 243 | |
aldo240498 | 0:aa2dd4e13c71 | 244 | } |
aldo240498 | 0:aa2dd4e13c71 | 245 | } |
aldo240498 | 0:aa2dd4e13c71 | 246 | // penyiangan |
aldo240498 | 0:aa2dd4e13c71 | 247 | if(data == 'b') //arm turun |
aldo240498 | 0:aa2dd4e13c71 | 248 | { |
aldo240498 | 0:aa2dd4e13c71 | 249 | // 0 ~ 180 |
aldo240498 | 0:aa2dd4e13c71 | 250 | for(int i=100; i<=180; i++) |
aldo240498 | 0:aa2dd4e13c71 | 251 | { |
aldo240498 | 0:aa2dd4e13c71 | 252 | int16_t Angle = 600 + (i * 10); |
aldo240498 | 0:aa2dd4e13c71 | 253 | Servo0.pulsewidth_us(Angle); |
aldo240498 | 0:aa2dd4e13c71 | 254 | //Servo1.pulsewidth_us(Angle); |
aldo240498 | 0:aa2dd4e13c71 | 255 | Servo5.pulsewidth_us(Angle); |
aldo240498 | 0:aa2dd4e13c71 | 256 | |
aldo240498 | 0:aa2dd4e13c71 | 257 | } |
aldo240498 | 0:aa2dd4e13c71 | 258 | } |
aldo240498 | 0:aa2dd4e13c71 | 259 | if(data == 'c') //arm naik |
aldo240498 | 0:aa2dd4e13c71 | 260 | { |
aldo240498 | 0:aa2dd4e13c71 | 261 | |
aldo240498 | 0:aa2dd4e13c71 | 262 | // 180 ~ 0 |
aldo240498 | 0:aa2dd4e13c71 | 263 | for(int i=180; i>=100; i--) |
aldo240498 | 0:aa2dd4e13c71 | 264 | { |
aldo240498 | 0:aa2dd4e13c71 | 265 | int16_t Angle = 600 + (i * 10); |
aldo240498 | 0:aa2dd4e13c71 | 266 | Servo0.pulsewidth_us(Angle); |
aldo240498 | 0:aa2dd4e13c71 | 267 | //Servo1.pulsewidth_us(Angle); |
aldo240498 | 0:aa2dd4e13c71 | 268 | Servo5.pulsewidth_us(Angle); |
aldo240498 | 0:aa2dd4e13c71 | 269 | } |
aldo240498 | 0:aa2dd4e13c71 | 270 | } |
aldo240498 | 0:aa2dd4e13c71 | 271 | if(data == 'e') //tutup |
aldo240498 | 0:aa2dd4e13c71 | 272 | { |
aldo240498 | 0:aa2dd4e13c71 | 273 | |
aldo240498 | 0:aa2dd4e13c71 | 274 | // 180 ~ 0 |
aldo240498 | 0:aa2dd4e13c71 | 275 | for(int i=180; i>=0; i--) |
aldo240498 | 0:aa2dd4e13c71 | 276 | { |
aldo240498 | 0:aa2dd4e13c71 | 277 | int16_t Angle = 600 + (i * 10); |
aldo240498 | 0:aa2dd4e13c71 | 278 | Servo2.pulsewidth_us(Angle); |
aldo240498 | 0:aa2dd4e13c71 | 279 | //Servo3.pulsewidth_us(Angle); |
aldo240498 | 0:aa2dd4e13c71 | 280 | Servo4.pulsewidth_us(Angle); |
aldo240498 | 0:aa2dd4e13c71 | 281 | |
aldo240498 | 0:aa2dd4e13c71 | 282 | } |
aldo240498 | 0:aa2dd4e13c71 | 283 | } |
aldo240498 | 0:aa2dd4e13c71 | 284 | if(data == 'd') //buka |
aldo240498 | 0:aa2dd4e13c71 | 285 | { |
aldo240498 | 0:aa2dd4e13c71 | 286 | |
aldo240498 | 0:aa2dd4e13c71 | 287 | // 180 ~ 0 |
aldo240498 | 0:aa2dd4e13c71 | 288 | for(int i=0; i<=110; i++) |
aldo240498 | 0:aa2dd4e13c71 | 289 | { |
aldo240498 | 0:aa2dd4e13c71 | 290 | int16_t Angle = 600 + (i * 10); |
aldo240498 | 0:aa2dd4e13c71 | 291 | Servo2.pulsewidth_us(Angle); |
aldo240498 | 0:aa2dd4e13c71 | 292 | //Servo3.pulsewidth_us(Angle); |
aldo240498 | 0:aa2dd4e13c71 | 293 | Servo4.pulsewidth_us(Angle); |
aldo240498 | 0:aa2dd4e13c71 | 294 | |
aldo240498 | 0:aa2dd4e13c71 | 295 | } |
aldo240498 | 0:aa2dd4e13c71 | 296 | } |
aldo240498 | 0:aa2dd4e13c71 | 297 | |
aldo240498 | 0:aa2dd4e13c71 | 298 | |
aldo240498 | 0:aa2dd4e13c71 | 299 | switch(data){ |
aldo240498 | 0:aa2dd4e13c71 | 300 | case'2': |
aldo240498 | 0:aa2dd4e13c71 | 301 | atur(50,0,0); //maju |
aldo240498 | 0:aa2dd4e13c71 | 302 | break; |
aldo240498 | 0:aa2dd4e13c71 | 303 | case'3': |
aldo240498 | 0:aa2dd4e13c71 | 304 | atur(-50,0,0); //mundur |
aldo240498 | 0:aa2dd4e13c71 | 305 | break; |
aldo240498 | 0:aa2dd4e13c71 | 306 | case'7': |
aldo240498 | 0:aa2dd4e13c71 | 307 | atur(0,-50,0); //kanan |
aldo240498 | 0:aa2dd4e13c71 | 308 | break; |
aldo240498 | 0:aa2dd4e13c71 | 309 | case'1': |
aldo240498 | 0:aa2dd4e13c71 | 310 | atur(0,50,0); //kiri |
aldo240498 | 0:aa2dd4e13c71 | 311 | break; |
aldo240498 | 0:aa2dd4e13c71 | 312 | case'5': |
aldo240498 | 0:aa2dd4e13c71 | 313 | atur(0,0,50); //muter kanan |
aldo240498 | 0:aa2dd4e13c71 | 314 | break; |
aldo240498 | 0:aa2dd4e13c71 | 315 | case'8': |
aldo240498 | 0:aa2dd4e13c71 | 316 | atur(0,0,-50); //muter kiri |
aldo240498 | 0:aa2dd4e13c71 | 317 | break; |
aldo240498 | 0:aa2dd4e13c71 | 318 | case'9': |
aldo240498 | 0:aa2dd4e13c71 | 319 | { |
aldo240498 | 0:aa2dd4e13c71 | 320 | for(double posisi=0; posisi<=329.700012; posisi++) |
aldo240498 | 0:aa2dd4e13c71 | 321 | { |
aldo240498 | 0:aa2dd4e13c71 | 322 | posisi=compass.readBearing()/10.0; |
aldo240498 | 0:aa2dd4e13c71 | 323 | printf("Bearing is: %f\n", posisi); |
aldo240498 | 0:aa2dd4e13c71 | 324 | atur(0,0,100); |
aldo240498 | 0:aa2dd4e13c71 | 325 | if(posisi==329.700012); |
aldo240498 | 0:aa2dd4e13c71 | 326 | } |
aldo240498 | 0:aa2dd4e13c71 | 327 | atur(0,0,0); |
aldo240498 | 0:aa2dd4e13c71 | 328 | |
aldo240498 | 0:aa2dd4e13c71 | 329 | |
aldo240498 | 0:aa2dd4e13c71 | 330 | default: |
aldo240498 | 0:aa2dd4e13c71 | 331 | dir1 = 1; |
aldo240498 | 0:aa2dd4e13c71 | 332 | dir2 = 1; |
aldo240498 | 0:aa2dd4e13c71 | 333 | dir3 = 1; |
aldo240498 | 0:aa2dd4e13c71 | 334 | dir4 = 1; |
aldo240498 | 0:aa2dd4e13c71 | 335 | dir5 = 1; |
aldo240498 | 0:aa2dd4e13c71 | 336 | dir6 = 1; |
aldo240498 | 0:aa2dd4e13c71 | 337 | dir7 = 1; |
aldo240498 | 0:aa2dd4e13c71 | 338 | dir8 = 1; |
aldo240498 | 0:aa2dd4e13c71 | 339 | ; |
aldo240498 | 0:aa2dd4e13c71 | 340 | |
aldo240498 | 0:aa2dd4e13c71 | 341 | |
aldo240498 | 0:aa2dd4e13c71 | 342 | } |
aldo240498 | 0:aa2dd4e13c71 | 343 | /* |
aldo240498 | 0:aa2dd4e13c71 | 344 | if(data == '3') //mundur |
aldo240498 | 0:aa2dd4e13c71 | 345 | { |
aldo240498 | 0:aa2dd4e13c71 | 346 | atur(100,0,0); |
aldo240498 | 0:aa2dd4e13c71 | 347 | } |
aldo240498 | 0:aa2dd4e13c71 | 348 | |
aldo240498 | 0:aa2dd4e13c71 | 349 | if(data == '7') //kanan |
aldo240498 | 0:aa2dd4e13c71 | 350 | { |
aldo240498 | 0:aa2dd4e13c71 | 351 | atur(0,100,0); |
aldo240498 | 0:aa2dd4e13c71 | 352 | } |
aldo240498 | 0:aa2dd4e13c71 | 353 | |
aldo240498 | 0:aa2dd4e13c71 | 354 | if(data == '1') //kiri |
aldo240498 | 0:aa2dd4e13c71 | 355 | { |
aldo240498 | 0:aa2dd4e13c71 | 356 | atur(0,-100,0); |
aldo240498 | 0:aa2dd4e13c71 | 357 | } |
aldo240498 | 0:aa2dd4e13c71 | 358 | if(data == '4') //armkanan |
aldo240498 | 0:aa2dd4e13c71 | 359 | { |
aldo240498 | 0:aa2dd4e13c71 | 360 | atur(0,0,150); |
aldo240498 | 0:aa2dd4e13c71 | 361 | } |
aldo240498 | 0:aa2dd4e13c71 | 362 | if(data == '8') //armkanan |
aldo240498 | 0:aa2dd4e13c71 | 363 | { |
aldo240498 | 0:aa2dd4e13c71 | 364 | atur(0,0,-150); |
aldo240498 | 0:aa2dd4e13c71 | 365 | } |
aldo240498 | 0:aa2dd4e13c71 | 366 | if(data == 'r') //armkanan |
aldo240498 | 0:aa2dd4e13c71 | 367 | { |
aldo240498 | 0:aa2dd4e13c71 | 368 | atur(0,0,0); |
aldo240498 | 0:aa2dd4e13c71 | 369 | } |
aldo240498 | 0:aa2dd4e13c71 | 370 | */ |
aldo240498 | 0:aa2dd4e13c71 | 371 | |
aldo240498 | 0:aa2dd4e13c71 | 372 | |
aldo240498 | 0:aa2dd4e13c71 | 373 | // untuk mode pemotong menggunakan relay |
aldo240498 | 0:aa2dd4e13c71 | 374 | if(data == 'a'){ |
aldo240498 | 0:aa2dd4e13c71 | 375 | relay1=1; |
aldo240498 | 0:aa2dd4e13c71 | 376 | |
aldo240498 | 0:aa2dd4e13c71 | 377 | } |
aldo240498 | 0:aa2dd4e13c71 | 378 | if(data == 'z') { |
aldo240498 | 0:aa2dd4e13c71 | 379 | relay1=0; |
aldo240498 | 0:aa2dd4e13c71 | 380 | } |
aldo240498 | 0:aa2dd4e13c71 | 381 | |
aldo240498 | 0:aa2dd4e13c71 | 382 | |
aldo240498 | 0:aa2dd4e13c71 | 383 | |
aldo240498 | 0:aa2dd4e13c71 | 384 | } |
aldo240498 | 0:aa2dd4e13c71 | 385 | |
aldo240498 | 0:aa2dd4e13c71 | 386 | |
aldo240498 | 0:aa2dd4e13c71 | 387 | |
aldo240498 | 0:aa2dd4e13c71 | 388 | }}}} |
aldo240498 | 0:aa2dd4e13c71 | 389 | |
aldo240498 | 0:aa2dd4e13c71 | 390 |