void set_motor_speed1( int speed )
{
if (speed>0)
mot1.ForwardM1( 0x80, speed );
else
mot1.BackwardM1( 0x80, -speed );
}
void set_motor_speed4( int speed )
{
if (speed>0)
mot2.ForwardM2( 0x80, speed );
else
mot2.BackwardM2( 0x80, -speed );
}
//==============================================================================
// MotorsDriver::RDrive
//==============================================================================
void MotorsDriver::LDrive(short sVal) {
if (sVal != LDrivePrev) {
LDrivePrev = sVal;
if (sVal >= 0)
RClaw.ForwardM2(PACKETS_ADDRESS, sVal);
else
RClaw.BackwardM2(PACKETS_ADDRESS, -sVal);
}
}
void setup_encoders()
{
uint8_t M1mode = 0x81;
uint8_t M2mode = 0x81;
uint8_t address= 0x80;
bool result = mot.SetM1EncoderMode( address, M1mode );
result = mot.SetM2EncoderMode( address, M2mode );
result = mot.ReadEncoderModes( address, M1mode, M2mode );
printf("Read EncoderModes m1,m2 = %x, %x\n", M1mode, M2mode );
}
/*
void* serial_interface(void* mParam) // serial port used for arduino connections & GPS.
{
LoadCell_SerialInterface* mFoot = (LoadCell_SerialInterface*) mParam;
static char app_name[12];
static char read_r[4];
static char no_write_w[4];
static char device_p[4];
static char read_[20];
static char no_write_[20];
static char device_[20];
sprintf(app_name, "./avisual");
sprintf(read_r, "-R");
sprintf(no_write_w, "-w");
sprintf(device_p, "-p");
sprintf(read_, "ascii");
sprintf(no_write_, "0");
//printf("mFoot=%p; left_foot=%p\n", mFoot, &left_foot);
if (mFoot == &left_foot) {
sprintf(device_, "/dev/ttyACM0");
mFoot->_cl_rx_dump = 1;
mFoot->_cl_rx_dump_ascii = 1;
mFoot->_cl_port = strdup("/dev/ttyACM0");
mFoot->_cl_tx_bytes = 0;
mFoot->left_foot = false;
} else {
mFoot->_cl_port = strdup("/dev/ttyACM1");
mFoot->_cl_rx_dump = 1;
mFoot->_cl_rx_dump_ascii = 1;
mFoot->_cl_tx_bytes = 0;
mFoot->left_foot = true;
//sprintf(device_, "/dev/ttyACM1");
}
char * argv[] = { app_name, read_r, read_, no_write_w, no_write_, device_p, device_ };
mFoot->serial_loadcell_main( 7, argv );
return NULL;
}
*/
void read_main_battery()
{
// mot.SetMainVoltages ( address, 100, 250 );
// mot.SetLogicVoltages( address, 100, 250 );
uint16_t volts1 = mot.ReadMainBatteryVoltage( 0x80 );
float volt = ((float)volts1)/10.;
printf("\nMain Battery board #1 = %6.1f volts\n", volt );
volts1 = mot.ReadLogicBatteryVoltage( 0x80 );
volt = ((float)volts1)/10.;
printf("\nLogic Battery board #1 = %6.1f volts\n", volts1 );
//mot.SetM1MaxCurrent(0x80, 20.0);
//mot.SetM2MaxCurrent(0x80, 20.0);
}
void read_encoders()
{
uint32_t enc1;
uint32_t enc2;
uint8_t status;
bool valid= false;
enc1 = mot.ReadEncM1(address, &status, &valid);
enc2 = mot.ReadEncM2(address, &status, &valid);
printf("Encoders m1,m2= %6.2f, %6.2f \n", 2.0*enc1/2047., 2.0*enc2/2047. );
bool result = mot.ReadEncoders( address, enc1, enc2);
printf("Encoders m1,m2= %6.2f, %6.2f \n", 2.0*enc1/2047., 2.0*enc2/2047. );
//result = mot.ReadISpeeds( address, uint32_t &ispeed1,uint32_t &ispeed2);
}
int main( int argc, char *argv[] )
{
print_args(argc,argv);
bool stop=false;
init_claws();
time_t t;
time(&t);
time_stamp = *(localtime(&t));
printf("====================== CLAW =========================\n");
if (argc>1) {
if (strcmp(argv[1], "stop")==0) {
stop = true;
printf("Stop!");
int count = 10;
while (count--) {
mot.ForwardM1 (address, 0);
mot.ForwardM2 (address, 0);
mot2.ForwardM1(address, 0);
mot2.ForwardM2(address, 0);
/* mot3.ForwardM1(address, 0);
mot3.ForwardM2(address, 0);
mot4.ForwardM1(address, 0);
mot4.ForwardM2(address, 0); */
usleep(1000000); // 0.1 sec
usleep(1000000); // 0.1 sec
}
exit(1);
}
}
while (1)
{
printf("\nForward:\n");
mot.ForwardM1(address, 64);
mot.ForwardM2(address, 64);
/* mot2.ForwardM1(address, 64);
mot2.ForwardM2(address, 64); */
usleep(1000000); // 0.1 sec
//usleep(1000000); // 0.1 sec
//read_main_battery();
printf("\nBackward:\n");
mot.BackwardM1(address, 64);
mot.BackwardM2(address, 64);
/* mot2.BackwardM1(address, 64);
mot2.BackwardM2(address, 64); */
usleep(1000000); // 0.1 sec
//usleep(1000000); // 0.1 sec
}
}
void init_claws()
{
if (mot.connected == true)
printf("Roboclaw #1 - Available\n" );
else printf("Roboclaw #1 - not available\n");
if (mot2.connected == true)
printf("Roboclaw #2 - Available\n" );
else printf("Roboclaw #2 - not available\n");
if (mot3.connected == true)
printf("Roboclaw #3 - Available\n" );
else printf("Roboclaw #3 - not available\n");
if (mot4.connected == true)
printf("Roboclaw #4 - Available\n" );
else printf("Roboclaw #4 - not available\n");
int maxCurrent = 20;
read_main_battery( );
//uint16_t config = 0x0067;
//mot.SetConfig ( address, config );
usleep(1000000); // 0.1 sec
//config = 0;
//bool result = mot.GetConfig( address, config );
//printf("Config=%4x\n", config);
bool valid=false;
uint16_t error = mot.ReadError(address, &valid);
printf("hip ErrorStatus = %4x %d\n\n", error, valid);
char text[80];
bool result = mot.ReadVersion( address, text );
printf("\ntext=%s\n", text);
//setup_encoders();
//read_encoders ();
/* mot.SetM1MaxCurrent(0x80, maxCurrent*100);
mot2.SetMainVoltages( address, 100, 250 );
mot3.SetMainVoltages( address, 100, 250 );
mot4.SetMainVoltages( address, 100, 250 ); */
}
void CheckVoltages(void)
{
#ifdef AVS_PIN
uint16_t wVS = analogRead(AVS_PIN);
#ifdef DEBUG
if (g_fDebugOutput && (wVS != g_wVSPrev)) {
Serial.print("VS: ");
Serial.println(wVS, DEC);
g_wVSPrev = wVS;
}
#endif
if ((wVS < (uint16_t)AVAL_MIN) && g_fServosInit) {
// detach any servos we may have...
MSound( 3, 100, 2500, 100, 2500, 100, 2500);
}
#elif defined(PACKET_MODE)
// Using Roboclaw in packet mode, so we can ask it for the
// motor voltages... Note: This requires us to send and receive
// stuff serially, so probably only do this every so often.
if ((millis()-g_ulCVTimeLast) > 1000)
{
// more than a second since we last tested
uint16_t wVS = RClaw.ReadMainBatteryVoltage(PACKETS_ADDRESS);
#ifdef DEBUG
if (g_fDebugOutput && (wVS != g_wVSPrev)) {
Serial.print("VS: ");
Serial.println(wVS, DEC);
g_wVSPrev = wVS;
}
#endif
if ((wVS < (uint16_t)VOLTAGE_MIN1) && g_fServosInit) {
// detach any servos we may have...
MSound( 3, 100, 2500, 100, 2500, 100, 2500);
}
g_ulCVTimeLast = millis(); // remember last we did it.
}
#endif
}
//--------------------------------------------------------------------------
// Main: the main function.
//--------------------------------------------------------------------------
int main(int argc, char *argv[])
{
// Install signal handler to allow us to do some cleanup...
struct sigaction sigIntHandler;
sigIntHandler.sa_handler = SignalHandler;
sigemptyset(&sigIntHandler.sa_mask);
sigIntHandler.sa_flags = 0;
sigaction(SIGINT, &sigIntHandler, NULL);
char abT[40]; // give a nice large buffer.
uint8_t cbRead;
printf("Start\n");
if (argc > 1)
{
for (int i=1; i < argc; i++)
{
printf("%d - %s\n", i, argv[i]);
}
}
char *pszDevice;
if (!RClaw.begin(pszDevice = (argc > 1? argv[1] : szRClawDevice), B38400))
{
printf("RClaw (%s) Begin failed\n", pszDevice);
return 0;
}
if (!command.begin(pszDevice = (argc > 2? argv[2] : szCommanderDevice), B38400))
{
printf("Commander (%s) Begin failed\n", pszDevice);
return 0;
}
int error;
delay(250);
Serial.begin(/*57600*/);
// Try to load the Rover Configuration Data
rcd.Load();
g_MotorsDriver.Init();
Serial.println("Kurt's Rover Program Startup\n");
g_fDebugOutput = false; // start with it off!
g_fShowDebugPrompt = true;
g_fRoverActive = false;
g_fRoverActivePrev = false;
g_fServosInit = false;
g_bGear = 3; // We init in 3rd gear.
g_bSteeringMode = ONE_STICK_MODE;
// Initialize our pan and tilt servos
InitializeServos(); // Make sure the servos are active
for(;;)
{
//--------------------------------------------------------------------------
// Loop: the main arduino main Loop function
//--------------------------------------------------------------------------
// We also have a simple debug monitor that allows us to
// check things. call it here..
if (TerminalMonitor())
continue;
CheckVoltages(); // check voltages - if we get too low shut down the servos...
// Lets get the PS2 data...
ControlInput();
// Drive the rover
if (g_fRoverActive) {
if (g_bSteeringMode == TANK_MODE) {
sRDrivePWM = LStickY; //RStickY; // BUGBUG - appears like wrong ones doing each...
sLDrivePWM = RStickY; // LStickY;
} else { // One stick driving
if ((RStickY >=0) && (RStickX >= 0)) { // Quadrant 1
sRDrivePWM = RStickY - RStickX;
sLDrivePWM = max(RStickX, RStickY);
} else if ((RStickY<0) && (RStickX>=0)) { //Quadrant 2
sRDrivePWM = (RStickY + RStickX);
sLDrivePWM = min (-RStickX, RStickY);
} else if ((RStickY<0) && (RStickX<0)) { //Quadrant 3
sRDrivePWM = min (RStickX, RStickY);
sLDrivePWM = (RStickY - RStickX);
} else if ((RStickY>=0) && (RStickX<0)) { // Quadrant 4
sRDrivePWM = max(-RStickX, RStickY);
sLDrivePWM = (RStickY + RStickX);
} else {
sRDrivePWM = 0;
sLDrivePWM = 0;
}
}
// Lets output the appropriate stuff to the motor controller
// ok lets figure out our speeds to output to the two motors. two different commands
// depending on going forward or backward.
// Scale the two values for the motors.
sRDrivePWM = max(min((sRDrivePWM * g_bGear) / 4, 127), -127); // This should keep up in the -127 to +127 range and scale it depending on what gear we are in.
sLDrivePWM = max(min((sLDrivePWM * g_bGear) / 4, 127), -127);
#ifdef DEBUG
if (g_fDebugOutput) {
if ((RStickY != RStickYPrev) || (RStickX != RStickXPrev) ||
(LStickY != LStickYPrev) || (LStickX != LStickXPrev) ||
(sRDrivePWM != sRDrivePWMPrev) || (sLDrivePWM != sLDrivePWMPrev)) {
Serial.print(LStickY, DEC);
Serial.print(",");
Serial.print(LStickX, DEC);
Serial.print(" ");
Serial.print(RStickY, DEC);
Serial.print(",");
Serial.print(RStickX, DEC);
Serial.print(" - ");
Serial.print(sLDrivePWM, DEC);
Serial.print(",");
Serial.println(sRDrivePWM, DEC);
LStickYPrev = LStickY;
LStickXPrev = LStickX;
RStickYPrev = RStickY;
RStickXPrev = RStickX;
sRDrivePWMPrev = sRDrivePWM;
sLDrivePWMPrev = sLDrivePWM;
}
}
#endif
// Call our motors driver code which may change depending on how we talk to the motors...
g_MotorsDriver.RDrive(sRDrivePWM);
g_MotorsDriver.LDrive(sLDrivePWM);
// Also if we have a pan/tilt lets update that as well..
#ifdef BBB_SERVO_SUPPORT
if (g_bSteeringMode != TANK_MODE) {
if (LStickX ) {
if (command.buttons & BUT_L6) { //modify which thing we are controlling depending on if L6 is down or not.
w = max(min(g_wRot + LStickX/8, rcd.aServos[RoverConfigData::ROTATE].wMax), rcd.aServos[RoverConfigData::ROTATE].wMin);
if (w != g_wRot) {
pinRot.SetDutyUS(w);
g_wRot = w;
}
} else {
w = max(min(g_wPan + LStickX/8, rcd.aServos[RoverConfigData::PAN].wMax), rcd.aServos[RoverConfigData::PAN].wMin);
if (w != g_wPan) {
pinPan.SetDutyUS(w);
g_wPan = w;
}
}
}
if (LStickY) {
w = max(min(g_wTilt + LStickY/8, rcd.aServos[RoverConfigData::TILT].wMax), rcd.aServos[RoverConfigData::TILT].wMin);
if (w != g_wTilt) {
pinTilt.SetDutyUS(w);
g_wTilt = w;
}
}
}
#endif
delay (10);
} else {
if (g_fRoverActivePrev) {
MSound( 3, 100, 2500, 80, 2250, 60, 2000);
g_MotorsDriver.RDrive(0);
g_MotorsDriver.LDrive(0);
}
delay (10);
}
g_fRoverActivePrev = g_fRoverActive;
}
}
