robotcar.c
3.43 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
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
#include <stdio.h>
#include <stdlib.h>
#include "robotcar.h"
#include "periph/pwm.h"
#include "periph/gpio.h"
#include "xtimer.h"
#ifndef MOTOR_FREQUENCY
#define MOTOR_FREQUENCY (1000U)
#endif
#ifndef MOTOR_RESOLUTION
#define MOTOR_RESOLUTION (1000U)
#endif
int robot_init(robot_t *dev, pwm_t pwm, int pwm_channel, gpio_t in1, gpio_t in2, gpio_t stby, gpio_t encoder, int id)
{
int actual_frequency;
actual_frequency = pwm_init(pwm, PWM_LEFT, MOTOR_FREQUENCY, MOTOR_RESOLUTION);
//printf("motor: 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->encoder = encoder;
dev->stby = stby;
dev->counter = 0;
dev->id = id;
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;}
if(gpio_init(dev->encoder,GPIO_OUT) < 0){return -1;}
return 0;
}
void robot_step_counter( robot_t *dev){
dev->counter++;
printf("step motor %d: %d \n",dev->id,dev->counter);
}
int cm_to_steps(uint8_t cm){
int result;
float circumference = (WHEEL_DIAM * 3.14)/10;
float cm_step = circumference / STEP_COUNT;
float f_result = cm/cm_step;
result = (int)f_result;
return result;
}
void robot_fw( robot_t *dev, uint16_t speed){
gpio_set(dev->in1);
gpio_clear(dev->in2);
pwm_set(dev->device,dev->channel, speed);
}
void robot_rev( robot_t *dev, uint16_t speed){
gpio_clear(dev->in1);
gpio_set(dev->in2);
pwm_set(dev->device,dev->channel,speed);
}
void robot_drive( robot_t *dev, uint16_t speed, int direction){
gpio_set(dev->stby);
if(direction < 0) robot_fw(dev,speed);
else robot_rev(dev,speed);
}
void robot_stop(robot_t *dev1, robot_t *dev2){
gpio_set(dev1->stby);
gpio_clear(dev1->in1);
gpio_clear(dev1->in2);
gpio_clear(dev2->in1);
gpio_clear(dev2->in2);
dev1->counter = 0;
dev2->counter = 0;
}
void robot_move_forward( robot_t *dev1, robot_t *dev2, uint16_t speed,int steps){
while((steps > dev1->counter)&&(steps > dev2->counter))
{
if(steps > dev1->counter)robot_drive(dev1,speed,1);
if(steps > dev2->counter)robot_drive(dev2,speed,1);
}
robot_stop(dev1,dev2);
}
void robot_move_reverse( robot_t *dev1, robot_t *dev2, uint16_t speed,int steps){
while((steps > dev1->counter)&&(steps > dev2->counter))
{
if(steps > dev1->counter)robot_drive(dev1,speed,-1);
if(steps > dev2->counter)robot_drive(dev2,speed,-1);
}
robot_stop(dev2,dev1);
}
void robot_spin_right( robot_t *dev1, robot_t *dev2, uint16_t speed,int steps){
while((steps > dev1->counter)&&(steps > dev2->counter))
{
if(steps > dev1->counter)robot_drive(dev1,speed,-1);
if(steps > dev2->counter)robot_drive(dev2,speed,1);
}
robot_stop(dev1,dev2);
}
void robot_spin_left( robot_t *dev1, robot_t *dev2, uint16_t speed,int steps){
while((steps > dev1->counter)&&(steps > dev2->counter))
{
if(steps > dev1->counter)robot_drive(dev1,speed,1);
if(steps > dev2->counter)robot_drive(dev2,speed,-1);
}
robot_stop(dev1,dev2);
}