123456789101112131415161718192021222324252627282930313233343536373839404142434445464748 |
- #include <stdio.h>
- #include <stdint.h>
- #include "assert.h"
- #include "periph/gpio.h"
- #include "timex.h"
- #include "roomba.h"
- #include "board.h"
- #define RADIUS_STRAIGHT 0x8000
- void motors_drive(int16_t speed, int16_t radius)
- {
- uint8_t data[4];
- if ((speed > 500) || (speed < (0 - 500)) || (radius > 2000) || (radius < (0 - 2000)))
- motors_stop();
- data[0] = ((uint16_t)speed) & 0xFF;
- data[1] = (((uint16_t)speed) & 0xFF00) >> 8;
- if ((radius == 0) && (speed != 0))
- radius = RADIUS_STRAIGHT;
- data[2] = ((uint16_t)radius) & 0xFF;
- data[3] = (((uint16_t)radius) & 0xFF00) >> 8;
- roomba_cmd(ROOMBA_DRIVE, data, 4);
- }
- void motors_stop(void)
- {
- motors_drive(0, 0);
- motors_clean_set(0,0,0);
- }
- void motors_spin(int cw)
- {
- if(cw)
- motors_drive(10, -1);
- else
- motors_drive(10, 1);
- }
- void motors_clean_set(int brush, int vac, int side)
- {
- uint8_t data;
- data = ((!!brush) << 2) | ((!!vac) << 1) | (!!side);
- roomba_cmd(ROOMBA_MOTORS, &data, 1);
- }
|