tb6612fng.c
2.37 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
#include <stdio.h>
#include <stdlib.h>
#include "tb6612fng.h"
#include "periph/pwm.h"
#include "periph/gpio.h"
#include "timex.h" /* for US_PER_SEC */
#include "xtimer.h"
#define ENABLE_DEBUG (0)
#include "debug.h"
#ifndef MOTOR_FREQUENCY
#define MOTOR_FREQUENCY (1000U)
#endif
#ifndef MOTOR_RESOLUTION
#define MOTOR_RESOLUTION MOTOR_FREQUENCY
#endif
int motor_init(motor_t *dev, pwm_t pwm, int pwm_channel, gpio_t in1, gpio_t in2, gpio_t stby, int offset)
{
int actual_frequency;
actual_frequency = pwm_init(pwm, PWM_LEFT, MOTOR_FREQUENCY, MOTOR_RESOLUTION);
DEBUG("servo: requested %d hz, got %d hz\n", MOTOR_FREQUENCY, actual_frequency);
if (actual_frequency < 0) {
/* PWM error */
return -1;
}
dev->device = pwm;
dev->channel = pwm_channel;
dev->in1 = in1;
dev->in2 = in2;
dev->stby = stby;
dev->offset = offset;
if(gpio_init(dev->in1,GPIO_OUT) < 0) {return -1;}
if(gpio_init(dev->in2,GPIO_OUT) < 0){return -1;}
if(gpio_init(dev->stby,GPIO_OUT) < 0){return -1;}
return 0;
}
void motor_fw(const motor_t *dev, uint16_t speed){
gpio_set(dev->in1);
gpio_clear(dev->in2);
pwm_set(dev->device,dev->channel, speed);
}
void motor_rev(const motor_t *dev, uint16_t speed){
gpio_clear(dev->in1);
gpio_set(dev->in2);
pwm_set(dev->device,dev->channel,speed);
}
void motor_drive(const motor_t *dev, uint16_t speed, int direction){
gpio_set(dev->stby);
speed = speed * dev->offset;
if(direction > 0) motor_fw(dev,speed);
else motor_rev(dev,speed);
}
void motor_brake(const motor_t *dev1, motor_t *dev2){
gpio_set(dev1->in1);
gpio_set(dev1->in2);
pwm_poweroff(dev1->device);
gpio_set(dev2->in1);
gpio_set(dev2->in2);
pwm_poweroff(dev2->device);
}
void motor_forward(const motor_t *dev1, const motor_t *dev2, uint16_t speed){
motor_drive(dev1,speed,1);
motor_drive(dev2,speed,1);
}
void motor_back(const motor_t *dev1, const motor_t *dev2, uint16_t speed){
motor_drive(dev1,speed,-1);
motor_drive(dev2,speed,-1);
}
void motor_left(const motor_t *left, const motor_t *right, uint16_t speed){
motor_drive(left,speed/2,-1);
motor_drive(right,speed/2,1);
}
void motor_right(const motor_t *left,const motor_t *right, uint16_t speed){
motor_drive(left,speed/2,1);
motor_drive(right,speed/2,-1);
}
void motor_stby(const motor_t *dev){
gpio_clear(dev->stby);
}