123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113 |
- //! Open loop motor control
- #![deny(unsafe_code)]
- #![deny(warnings)]
- #![no_main]
- #![no_std]
- extern crate cortex_m_rt as rt;
- extern crate cortex_m_semihosting as sh;
- extern crate motor_driver;
- extern crate panic_semihosting;
- #[macro_use(block)]
- extern crate nb;
- extern crate stm32f1xx_hal as hal;
- use core::fmt::Write;
- use hal::prelude::*;
- use hal::serial::Serial;
- use hal::stm32f103xx;
- use motor_driver::Motor;
- use rt::{entry, exception, ExceptionFrame};
- use sh::hio;
- #[entry]
- fn main() -> ! {
- let p = stm32f103xx::Peripherals::take().unwrap();
- let mut flash = p.FLASH.constrain();
- let mut rcc = p.RCC.constrain();
- let clocks = rcc.cfgr.freeze(&mut flash.acr);
- let mut afio = p.AFIO.constrain(&mut rcc.apb2);
- let mut gpioa = p.GPIOA.split(&mut rcc.apb2);
- let tx = gpioa.pa9.into_alternate_push_pull(&mut gpioa.crh);
- let rx = gpioa.pa10;
- let serial = Serial::usart1(
- p.USART1,
- (tx, rx),
- &mut afio.mapr,
- 115_200.bps(),
- clocks,
- &mut rcc.apb2,
- );
- let mut rx = serial.split().1;
- let pwm = p.TIM2.pwm(
- gpioa.pa0.into_alternate_push_pull(&mut gpioa.crl),
- &mut afio.mapr,
- 1.khz(),
- clocks,
- &mut rcc.apb1,
- );
- let max_duty = pwm.get_max_duty() as i16;
- let mut motor = Motor::tb6612fng(
- gpioa.pa1.into_push_pull_output(&mut gpioa.crl),
- gpioa.pa2.into_push_pull_output(&mut gpioa.crl),
- pwm,
- );
- let mut duty = max_duty;
- let mut brake = true;
- motor.duty(duty as u16);
- let mut hstdout = hio::hstdout().unwrap();
- writeln!(hstdout, "{} {}", max_duty, brake).unwrap();
- loop {
- match block!(rx.read()).unwrap() {
- b'*' => duty *= 2,
- b'+' => duty += 1,
- b'-' => duty -= 1,
- b'/' => duty /= 2,
- b'r' => duty *= -1,
- b's' => brake = !brake,
- _ => continue,
- }
- if duty > max_duty {
- duty = max_duty;
- } else if duty < -max_duty {
- duty = -max_duty;
- }
- if brake {
- motor.brake();
- } else if duty > 0 {
- motor.cw();
- } else {
- motor.ccw();
- }
- motor.duty(duty.abs() as u16);
- writeln!(hstdout, "{} {}", duty, brake).unwrap();
- }
- }
- #[exception]
- fn HardFault(ef: &ExceptionFrame) -> ! {
- panic!("{:#?}", ef);
- }
- #[exception]
- fn DefaultHandler(irqn: i16) {
- panic!("Unhandled exception (IRQn = {})", irqn);
- }
|