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
|
#include <5106z.h>
#include <string.h>
typedef struct {
int8_t *speedP;
int8_t speed;
uint8_t thresh;
uint8_t polarity :1;
uint8_t enable :1;
uint8_t useptr :1;
uint8_t imeable :3;
} __attribute__ ((packed)) motor_t;
static motor_t motor[11];
void motor_init(uint8_t count,...){
va_list m;
uint8_t i;
memset(motor,0,11*sizeof(motor_t));
va_start(m,count);
for(i=0;i<count;i++){
motor[va_arg(m,int)].enable=true;
}
va_end(m);
}
void motor_initIMEs(uint8_t count,...){
va_list m;
uint8_t i;
va_start(m,count);
for(i=0;i<count;i++){
motor[va_arg(m,int)].imeable=i;
}
va_end(m);
}
int motor_imeReset(uint8_t port){
if(!motor[port].enable) return -1;
if(!motor[port].imeable)return -2;
imeReset(motor[port].imeable);
return 0;
}
int motor_imeRead(uint8_t port){
int imeValue;
if(!motor[port].enable) return -1;
if(!motor[port].imeable)return -2;
if(!imeGet(motor[port].imeable,&imeValue))return -3;
return imeValue;
}
void motor_enable(uint8_t port){
motor[port].enable=true;
}
void motor_disable(uint8_t port){
motor[port].enable=false;
}
int motor_togglePolarity(uint8_t port){
if(!motor[port].enable)return -1;
return (motor[port].polarity^=true);
}
int motor_setSpeed(uint8_t port,int8_t speed){
if(!motor[port].enable)return -1;
return (motor[port].speed=abs(speed)>motor[port].thresh?speed:0);
}
int motor_setSpeedSum(uint8_t port,uint8_t args,...){
va_list s;
uint8_t i;
int8_t speed=0;
if(!motor[port].enable)return -1;
va_start(s,args);
for(i=0;i<args;i++){
speed+=va_arg(s,int);
}
va_end(s);
return motor_setSpeed(port,speed);
}
int motor_setSpeedPointer(uint8_t port,int8_t *sp){
if(!motor[port].enable)return -1;
motor[port].useptr=true;
motor[port].speedP=sp;
return 0;
}
int motor_setThreshold(uint8_t port,uint8_t threshold){
if(!motor[port].enable)return -1;
return (motor[port].thresh=threshold);
}
int motor_applySpeeds(void){
uint8_t i;
for(i=0;i<10;i++){
if(motor[i].enable){
if(motor[i].useptr)
motor[i].speed=*motor[i].speedP;
motorSet(i,motor[i].speed*(motor[i].polarity?-1:1));
}
}
return 0;
}
|
