50 lines
1.1 KiB
C++
50 lines
1.1 KiB
C++
#include "serialPort/SerialPort.h"
|
|
#include <csignal>
|
|
|
|
int main(){
|
|
// set the serial port name
|
|
SerialPort serial("/dev/ttyUSB0");
|
|
|
|
// send message struct
|
|
MOTOR_send motor_run, motor_stop;
|
|
// receive message struct
|
|
MOTOR_recv motor_r;
|
|
|
|
// set the id of motor
|
|
motor_run.id = 0;
|
|
// set the motor type, A1 or B1
|
|
motor_run.motorType = MotorType::A1;
|
|
motor_run.mode = 5;
|
|
motor_run.T = 0.0;
|
|
motor_run.W = 0.0;
|
|
motor_run.Pos = 0.0;
|
|
motor_run.K_P = 0.0;
|
|
motor_run.K_W = 0.0;
|
|
|
|
motor_stop.id = motor_run.id;
|
|
motor_stop.motorType = motor_run.motorType;
|
|
motor_stop.mode = 0;
|
|
|
|
motor_r.motorType = motor_run.motorType;
|
|
|
|
// encode data into motor commands
|
|
modify_data(&motor_run);
|
|
modify_data(&motor_stop);
|
|
|
|
// turn for 3 second
|
|
for(int i(0); i<3; ++i){
|
|
serial.sendRecv(&motor_run, &motor_r);
|
|
// decode data from motor states
|
|
extract_data(&motor_r);
|
|
std::cout << "Pos: " << motor_r.Pos << std::endl;
|
|
usleep(1000000);
|
|
}
|
|
|
|
// stop the motor
|
|
while(!serial.sendRecv(&motor_stop, &motor_r)){
|
|
usleep(100000);
|
|
}
|
|
|
|
return 0;
|
|
}
|