robocon_2016
/
2016_slave_servo
servo_/30
Diff: main.cpp
- Revision:
- 0:f7be77a7663f
--- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/main.cpp Fri Sep 30 12:33:22 2016 +0000 @@ -0,0 +1,53 @@ +#include "mbed.h" +#include "i2cslave.h" +#include "define.h" + +char Registar[128]= {}; +void check(); + +int main() +{ + PwmOut Servos[4] = { PwmOut(dp24), PwmOut(dp1), PwmOut(dp2), PwmOut(dp18) }; + char servodata[4] = {}; + for(int i = 0; i < 4; i++) + { + Servos[i] = 0; + } + NVIC_SetPriority(TIMER_16_0_IRQn, 20); + NVIC_SetPriority(TIMER_16_1_IRQn, 20); + NVIC_SetPriority(TIMER_32_0_IRQn, 20); + NVIC_SetPriority(TIMER_32_1_IRQn, 20); + NVIC_SetPriority(I2C_IRQn, 10); + i2cslave i2c(dp5, dp27, Registar); + /*change address every micon***********/ + i2c.address(SERVO1_addr); + /**************************************/ + i2c.frequency(1000000); + Ticker tic; + tic.attach(check, 0.1); + while(1) + { + for(int i = 0; i < 4; i++) + { + servodata[i] = (Registar[motor1_state] >> i) & 1; + if(servodata[i]) + { + Servos[i].pulsewidth(0.0010); + } + else + { + Servos[i].pulsewidth(0.0006); + } + wait_ms(60); + } + } +} + +void check() +{ + Registar[check_reg]++; + if(Registar[check_reg] > 2) + { + NVIC_SystemReset(); + } +}