-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathmain_joy2.cpp
132 lines (111 loc) · 3.6 KB
/
main_joy2.cpp
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
// ジョイスティック２本
#include "mbed.h"
#include "QEI.h"
#include "PS3.h"
#include <cstdint> //uint8 (char) を使用したら勝手に追加された
#define M_R_WHEEL 0x66
#define M_L_WHEEL 0x64
#define M3ADDRESS 0x62
#define M4ADDRESS 0x60
Thread th1;
Thread th2;
int thread1();
int thread2();
I2C i2c(D14,D15); // D14 = PB_8, D15 = PB_9
PS3 ps3( A0, A1); // PA_0, PA_1
DigitalIn button(BUTTON1);
DigitalIn sw1(PC_4);
DigitalIn sw2(PB_15);
DigitalIn sw3(PB_12);
DigitalIn sw4(PB_15);
DigitalIn sw5(PC_8);
DigitalOut myled(LED1);
DigitalOut led1(PA_9); // PA_9 = D8
DigitalOut led2(PA_8); // PA_8 = D7
DigitalOut led3(PB_10); // PB_10 = D6
DigitalOut led4(PB_4); // PB_4 = D5
DigitalOut led5(PB_5); // PB_5 = D4
QEI rori1( PD_2, PC_11, NC, 2048, QEI::X2_ENCODING);
QEI rori2( PC_10, PC_12, NC, 2048, QEI::X2_ENCODING);
QEI rori3( PA_13, PA_14, NC, 2048, QEI::X2_ENCODING);
QEI rori4( PA_15, PB_7, NC, 2048, QEI::X2_ENCODING);
QEI rori5( PH_0, PH_1, NC, 2048, QEI::X2_ENCODING);
QEI rori6( PC_3, PA_4, NC, 2048, QEI::X2_ENCODING);
void send(char, char);
int outputMotorData_R_W (char *data);
int outputMotorData_L_W (char *data);
int pulse1, pulse2, pulse3, pulse4, pulse5, pulse6;
float angle1, angle2, angle3, angle4, angle5, angle6;
char R_W_Mdata, L_W_Mdata; //足回りのMDに送るPWMデータ
int main (void){
R_W_Mdata = 0x80;
L_W_Mdata = 0x80;
th1.start(thread1);
th2.start(thread2);
rori1.reset();
rori2.reset();
rori3.reset();
rori4.reset();
rori5.reset();
rori6.reset();
while (true){
outputMotorData_L_W(&L_W_Mdata);
outputMotorData_R_W(&R_W_Mdata);
send(M_R_WHEEL, 256 - L_W_Mdata);
send(M_L_WHEEL, R_W_Mdata);
send(M3ADDRESS, 0x80);
send(M4ADDRESS, 0x80);
}
}
int thread1 (void){
while (true){
// ps3.printdata();
// printf("lx:%2d ly:%2d la:%3.1f rx:%2d ry:%2d ra:%3.1f\n", ps3.getLeftJoystickXaxis(), ps3.getLeftJoystickYaxis(), ps3.getLeftJoystickAngle(), ps3.getRightJoystickXaxis(), ps3.getRightJoystickYaxis(), ps3.getRightJoystickAngle());
printf("Ly:%2d Ldata16:%2x Ldata10:%3d Ry:%2d Rdata16:%2x Rdata10:%3d \n", ps3.getLeftJoystickYaxis(), L_W_Mdata, L_W_Mdata, ps3.getRightJoystickYaxis(),R_W_Mdata, R_W_Mdata);
// printf("%d\n",Lx);
ThisThread::sleep_for(100ms);
}
}
int thread2 (void){
while (true){
pulse1 = rori1.getPulses();
pulse2 = rori2.getPulses();
pulse3 = rori3.getPulses();
pulse4 = rori4.getPulses();
pulse5 = rori5.getPulses();
pulse6 = rori6.getPulses();
angle1 = ( 360*(float)pulse1 / (2048*2) );
angle2 = ( 360*(float)pulse2 / (2048*2) );
angle3 = ( 360*(float)pulse3 / (2048*2) );
angle4 = ( 360*(float)pulse4 / (2048*2) );
angle5 = ( 360*(float)pulse5 / (2048*2) );
angle6 = ( 360*(float)pulse6 / (2048*2) );
}
}
void send (char address, char data){
i2c.start();
i2c.write(address);
i2c.write(data);
i2c.stop();
ThisThread::sleep_for(15ms);
}
int outputMotorData_R_W (char *data){
int Ry;
if ( ps3.getRightJoystickYaxis() == 64){
Ry = 64-1;
}else{
Ry = ps3.getRightJoystickYaxis();
}
*data = 128 + ( Ry * 2 );
return 0;
}
int outputMotorData_L_W (char *data){
int Ly;
if ( ps3.getLeftJoystickYaxis() == 64){
Ly = 64-1;
}else{
Ly = ps3.getLeftJoystickYaxis();
}
*data = 128 + ( Ly * 2 );
return 0;
}