-
Notifications
You must be signed in to change notification settings - Fork 1
/
Copy pathmain_test.cpp
executable file
·126 lines (114 loc) · 4.81 KB
/
main_test.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
#include <iostream>
#include <stdio.h>
#include "exceptions.h"
#include "comms.h"
#include "usbcomm.h"
#include "bluecomm.h"
#include "message.h"
#include "opcodes.h"
using namespace std;
void playTune(Comms &nxt){
unsigned short int C=523;
unsigned short int D=587;
unsigned short int E=659;
unsigned short int G=784;
unsigned short int freq=500;
Opcodes op(&nxt);
op.playTone(E,freq);
op.playTone(D,freq);
op.playTone(C,freq);
op.playTone(D,freq);
op.playTone(E,freq);
op.playTone(E,freq);
op.playTone(E,freq);
op.playTone(D,freq);
op.playTone(D,freq);
op.playTone(D,freq);
usleep(500000);
op.playTone(E,freq);
op.playTone(G,freq);
op.playTone(G,freq);
usleep(500000);
op.playTone(E,freq);
op.playTone(D,freq);
op.playTone(C,freq);
op.playTone(D,freq);
op.playTone(E,freq);
op.playTone(E,freq);
op.playTone(E,freq);
op.playTone(E,freq);
op.playTone(D,freq);
op.playTone(D,freq);
op.playTone(E,freq);
op.playTone(D,freq);
op.playTone(C,freq);
sleep(1);
}
void printBatteryLevel(Comms &nxt){
Opcodes op(&nxt);
printf("Battery level: %hu [mv]\n",op.getBatteryLevel());
}
int main(int argc, char *argv[])
{
//USBComm nxt;
BlueComm nxt;
try{
//nxt.connect("00:16:53:13:05:B2");
nxt.connect("00:16:53:17:B7:44");
//Use nxt.find when on USB
//nxt.find();
}catch(NxtEx &ex){
cout<<ex.toString()<<endl;
return -1;
}
try{
//playTune(nxt);
printBatteryLevel(nxt);
Opcodes op(&nxt);
unsigned char status;
MotorState ms;
int alti = 0;
int az = 0;
//for (int i=1; i<10; i++){
//az
//printf ("azimuth:\n");
//ms = op.getOutputState(0x00);
//cout<<"az: "<<int(ms.port)<<" "<<int(ms.power)<<" "<<int(ms.mode)<<" "<<int(ms.regMode)<<" "<<int(ms.turnRatio)<<" "<<int(ms.runState)<<" "<<ms.tachoLimit<<" "<<ms.tachoCount<<" "<<ms.blockTachoCount<<" "<<ms.rotationCount<<endl;
//SEBA: Working msg for az
//op.setOutputState(0x00,80,0x01,0x00,50,0x20,(4.667*i),true,&status);
op.setOutputState(0x00,80,0x01,0x00,0,0x20,(4.667*az),true,&status);
ms = op.getOutputState(0x00);
cout<<"port: "<<int(ms.port)<<" power: "<<int(ms.power)<<" mode:"<<int(ms.mode)<<" regMode: "<<int(ms.regMode)<<" turnRatio: "<<int(ms.turnRatio)<<" runState: "<<int(ms.runState)<<" tachoLimit: "<<ms.tachoLimit<<" tachoCount: "<<ms.tachoCount<<" blockTachoCount: "<<ms.blockTachoCount<<" rotationCount: "<<ms.rotationCount<<endl;
//el
printf(" angulo %d \n",alti);
op.setOutputState(0x01,-80,0x01,0x00,50,0x20,alti/5,true,&status);//con este muevo el motor conectado a B
op.setOutputState(0x02,80,0x01,0x00,50,0x20,alti/5,true,&status);//con este muevo el motor conectado a C
ms = op.getOutputState(0x01);
cout<<"port: "<<int(ms.port)<<" power: "<<int(ms.power)<<" mode:"<<int(ms.mode)<<" regMode: "<<int(ms.regMode)<<" turnRatio: "<<int(ms.turnRatio)<<" runState: "<<int(ms.runState)<<" tachoLimit: "<<ms.tachoLimit<<" tachoCount: "<<ms.tachoCount<<" blockTachoCount: "<<ms.blockTachoCount<<" rotationCount: "<<ms.rotationCount<<endl;
ms = op.getOutputState(0x02);
cout<<"port: "<<int(ms.port)<<" power: "<<int(ms.power)<<" mode:"<<int(ms.mode)<<" regMode: "<<int(ms.regMode)<<" turnRatio: "<<int(ms.turnRatio)<<" runState: "<<int(ms.runState)<<" tachoLimit: "<<ms.tachoLimit<<" tachoCount: "<<ms.tachoCount<<" blockTachoCount: "<<ms.blockTachoCount<<" rotationCount: "<<ms.rotationCount<<endl;
//}
/*op.setInputMode(0x00, LIGHT_ACTIVE, RAWMODE);
SensorState ss;
cout<<"Status: "<<(int)status<<endl;
sleep(1);
for(int i=0;i<10;i++){
sleep(1);
ms = op.getOutputState(0);
cout<<"a: "<<int(ms.port)<<" "<<int(ms.power)<<" "<<int(ms.mode)<<" "<<int(ms.regMode)<<" "<<int(ms.turnRatio)<<" "<<int(ms.runState)<<" "<<ms.tachoLimit<<" "<<ms.tachoCount<<" "<<ms.blockTachoCount<<" "<<ms.rotationCount<<endl;
op.resetMotorPosition(0,false);
//ss = op.getInputValues(0x00);
//cout<<"a: "<<int(ss.port)<<" "<<ss.valid<<" "<<ss.calibrated<<" "<<ss.type<<" "<<ss.mode<<" "<<ss.rawValue<<" "<<ss.normalizedValue<<" "<<ss.scaledValue<<" "<<ss.calibratedValue<<endl;
}*/
//shuts down the motor
//op.setOutputState(0x00,0,0x00,0x00,0,0x00,0,true,&status);
//op.setOutputState(0x01,0,0x00,0x00,0,0x00,0,true,&status);
//op.setOutputState(0x02,0,0x00,0x00,0,0x00,0,true,&status);
nxt.disconnect();
}catch(NxtEx &ex){
cout<<ex.toString()<<endl;
nxt.disconnect();
return -1;
}
return 0;
}