robocon_2016
/
2016_slave_servo
servo_/30
main.cpp@0:f7be77a7663f, 2016-09-30 (annotated)
- Committer:
- sgrsn
- Date:
- Fri Sep 30 12:33:22 2016 +0000
- Revision:
- 0:f7be77a7663f
servo_9/30
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
sgrsn | 0:f7be77a7663f | 1 | #include "mbed.h" |
sgrsn | 0:f7be77a7663f | 2 | #include "i2cslave.h" |
sgrsn | 0:f7be77a7663f | 3 | #include "define.h" |
sgrsn | 0:f7be77a7663f | 4 | |
sgrsn | 0:f7be77a7663f | 5 | char Registar[128]= {}; |
sgrsn | 0:f7be77a7663f | 6 | void check(); |
sgrsn | 0:f7be77a7663f | 7 | |
sgrsn | 0:f7be77a7663f | 8 | int main() |
sgrsn | 0:f7be77a7663f | 9 | { |
sgrsn | 0:f7be77a7663f | 10 | PwmOut Servos[4] = { PwmOut(dp24), PwmOut(dp1), PwmOut(dp2), PwmOut(dp18) }; |
sgrsn | 0:f7be77a7663f | 11 | char servodata[4] = {}; |
sgrsn | 0:f7be77a7663f | 12 | for(int i = 0; i < 4; i++) |
sgrsn | 0:f7be77a7663f | 13 | { |
sgrsn | 0:f7be77a7663f | 14 | Servos[i] = 0; |
sgrsn | 0:f7be77a7663f | 15 | } |
sgrsn | 0:f7be77a7663f | 16 | NVIC_SetPriority(TIMER_16_0_IRQn, 20); |
sgrsn | 0:f7be77a7663f | 17 | NVIC_SetPriority(TIMER_16_1_IRQn, 20); |
sgrsn | 0:f7be77a7663f | 18 | NVIC_SetPriority(TIMER_32_0_IRQn, 20); |
sgrsn | 0:f7be77a7663f | 19 | NVIC_SetPriority(TIMER_32_1_IRQn, 20); |
sgrsn | 0:f7be77a7663f | 20 | NVIC_SetPriority(I2C_IRQn, 10); |
sgrsn | 0:f7be77a7663f | 21 | i2cslave i2c(dp5, dp27, Registar); |
sgrsn | 0:f7be77a7663f | 22 | /*change address every micon***********/ |
sgrsn | 0:f7be77a7663f | 23 | i2c.address(SERVO1_addr); |
sgrsn | 0:f7be77a7663f | 24 | /**************************************/ |
sgrsn | 0:f7be77a7663f | 25 | i2c.frequency(1000000); |
sgrsn | 0:f7be77a7663f | 26 | Ticker tic; |
sgrsn | 0:f7be77a7663f | 27 | tic.attach(check, 0.1); |
sgrsn | 0:f7be77a7663f | 28 | while(1) |
sgrsn | 0:f7be77a7663f | 29 | { |
sgrsn | 0:f7be77a7663f | 30 | for(int i = 0; i < 4; i++) |
sgrsn | 0:f7be77a7663f | 31 | { |
sgrsn | 0:f7be77a7663f | 32 | servodata[i] = (Registar[motor1_state] >> i) & 1; |
sgrsn | 0:f7be77a7663f | 33 | if(servodata[i]) |
sgrsn | 0:f7be77a7663f | 34 | { |
sgrsn | 0:f7be77a7663f | 35 | Servos[i].pulsewidth(0.0010); |
sgrsn | 0:f7be77a7663f | 36 | } |
sgrsn | 0:f7be77a7663f | 37 | else |
sgrsn | 0:f7be77a7663f | 38 | { |
sgrsn | 0:f7be77a7663f | 39 | Servos[i].pulsewidth(0.0006); |
sgrsn | 0:f7be77a7663f | 40 | } |
sgrsn | 0:f7be77a7663f | 41 | wait_ms(60); |
sgrsn | 0:f7be77a7663f | 42 | } |
sgrsn | 0:f7be77a7663f | 43 | } |
sgrsn | 0:f7be77a7663f | 44 | } |
sgrsn | 0:f7be77a7663f | 45 | |
sgrsn | 0:f7be77a7663f | 46 | void check() |
sgrsn | 0:f7be77a7663f | 47 | { |
sgrsn | 0:f7be77a7663f | 48 | Registar[check_reg]++; |
sgrsn | 0:f7be77a7663f | 49 | if(Registar[check_reg] > 2) |
sgrsn | 0:f7be77a7663f | 50 | { |
sgrsn | 0:f7be77a7663f | 51 | NVIC_SystemReset(); |
sgrsn | 0:f7be77a7663f | 52 | } |
sgrsn | 0:f7be77a7663f | 53 | } |