libwallaby/src/motors_c.cpp at d34e96741e867ca79611821842603a0bdcc0c590 · kipr/libwallaby · GitHub

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
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
/*
 * motors_c.cpp
 *
 *  Created on: Nov 6, 2015
 *      Author: Joshua Southerland
 */

#include "wallaby/motors.h"
#include "wallaby/util.h"
#include "motor_p.hpp"

#include <iostream>
#include <cstdlib>
#include <math.h>

int get_motor_position_counter(int motor)
{
	return Private::get_motor_bemf(motor, nullptr);
}

int gmpc(int motor)
{
	return get_motor_position_counter(motor);
}

void clear_motor_position_counter(int motor)
{
	Private::clear_motor_bemf(motor);
}

void cmpc(int motor)
{
	clear_motor_position_counter(motor);
}

int move_at_velocity(int motor, int velocity)
{
	Private::set_motor_mode(motor, Private::Motor::Speed);
	Private::set_motor_goal_velocity(motor, velocity);
	return 0;
}

int mav(int motor, int velocity)
{
	return move_at_velocity(motor, velocity);
}

int move_to_position(int motor, int speed, int goal_pos)
{
	// FIXME: handle velocity scaling?
	const int sign = Private::get_motor_bemf(motor, nullptr) > goal_pos ? -1 : 1;
	const short velocity = std::abs(speed) * sign;

	// TODO: combine into one call
	Private::set_motor_goal_position(motor, goal_pos);
	Private::set_motor_goal_velocity(motor, velocity);
	Private::set_motor_mode(motor, Private::Motor::SpeedPosition);

	return 0;
}

int mtp(int motor, int speed, int goal_pos)
{
	return move_to_position(motor, speed, goal_pos);
}

int move_relative_position(int motor, int speed, int delta_pos)
{
	if (motor < 0 || motor > 3) return -1;
	move_to_position(motor, speed, Private::get_motor_bemf(motor, nullptr) + delta_pos);
	return 0;
}

int mrp(int motor, int speed, int delta_pos)
{
	return move_relative_position(motor, speed, delta_pos);
}

void set_pid_gains(int motor, short p, short i, short d, short pd, short id, short dd)
{
	Private::set_motor_pid_gains(motor, p, i, d, pd, id, dd);
}

void get_pid_gains(int motor, short * p, short * i, short * d, short * pd, short * id, short * dd)
{
	Private::get_motor_pid_gains(motor, *p, *i, *d, *pd, *id, *dd);
}

int freeze(int motor)
{
	Private::set_motor_pwm(motor, 100);
	Private::set_motor_direction(motor, Private::Motor::ActiveStop);
	return 0;
}

int get_motor_done(int motor)
{
	if (motor < 0 || motor > 3) return -1;
	msleep(2); // to make sure PID was run.. TODO: remove?
	return (Private::get_motor_pid_active(motor, nullptr) ? 0 : 1);
}

void block_motor_done(int motor)
{
	while (!get_motor_done(motor)) msleep(2);
}

void bmd(int motor)
{
	block_motor_done(motor);
}

int setpwm(int motor, int pwm)
{
	Private::set_motor_pwm(motor, pwm);
	return -1;
}

int getpwm(int motor)
{
	return -1;
}

void fd(int motor)
{
	::motor(motor, 100);
}

void bk(int motor)
{
	::motor(motor, -100);
}

void motor(int motor, int percent)
{
	mav(motor, percent*15); // 100 percent = 1500 ticks/sec
}

void baasbennaguui(int motor, int percent)
{
        mav(motor, percent*15); // 100 percent = 1500 ticks/sec
}

void motor_power(int motor, int percent)
{
	Private::set_motor_mode(motor, Private::Motor::Inactive);
	Private::set_motor_pwm(motor, std::abs(percent));

	if (percent > 0)
	{
		Private::set_motor_direction(motor, Private::Motor::Forward);
	}
	else if (percent < 0)
	{
		Private::set_motor_direction(motor, Private::Motor::Reverse);
	}
	else
	{
		Private::set_motor_direction(motor, Private::Motor::PassiveStop);
	}
}

void off(int motor)
{
	Private::stop_motor(motor);
}

void alloff()
{
	for (unsigned int i = 0; i < 4; ++i) off(i);
}

void ao()
{
	alloff();
}