My PhidgetSBC Car

Jacob
Phidgetly
Posts: 38
Joined: Tue Sep 29, 2009 8:56 pm
Contact:

My PhidgetSBC Car

Postby Jacob » Sun Apr 24, 2011 3:25 pm

I recently have been working on a Phidget SBC based car. I am using a SBC2, a Motor Controller HC, a Voltage Sensor, and a GPS

My plan was to have the SBC broadcast a Ad Hoc network (see here, and to connect an iPhone to that, I wrote an app, but had connection issues. I moved to PC.

My code was written to be compiled in MinGW with a modified phidget21.h (attached)

Code: Select all

#include "phidget21.h"
#include <iostream>
#include <windows.h>
#include <cstdio>
using namespace std;

CPhidgetGPSHandle gps;
CPhidgetInterfaceKitHandle ifkit;
CPhidgetMotorControlHandle motor;
bool gpsLocated;

int __stdcall ifkitadded(CPhidgetHandle phid, void *usrptr){
   printf("Interface Kit Connected!\n");
   return 0;
}

int __stdcall motoradded(CPhidgetHandle phid, void *usrptr){
   printf("Motor Controller Connected!\n");
   return 0;
}

int __stdcall gpsadded(CPhidgetHandle phid, void *usrptr){
   printf("GPS Connected!\n");
   return 0;
}

int __stdcall ifkitremoved(CPhidgetHandle phid, void *usrptr){
   printf("Interface Kit Disconnected!\n");
   return 0;
}

int __stdcall motorremoved(CPhidgetHandle phid, void *usrptr){
   printf("Motor Controller Disconnected!\n");
   return 0;
}

int __stdcall gpsremoved(CPhidgetHandle phid, void *usrptr){
   printf("GPS Disconnected!\n");
   return 0;
}

int __stdcall gpsmoved(CPhidgetGPSHandle phid, void *usrPtr, double latitude, double longitude, double altitude){
   double velocity;
   CPhidgetGPS_getVelocity(gps, &velocity);
   if (velocity < 10000){
   printf("Altitude: %.02f feet Velocity: %.02f mi/hr \n", altitude * 3.2808399, velocity * 0.62137119223733);
   }
   return 0;
}

int __stdcall ifkitchange(CPhidgetInterfaceKitHandle phid, void *usrptr, int index, int sensorValue){
   if (sensorValue / 13.62 - 36.7107 > -35){
   printf("Battery Voltage: %.02f volts \n", sensorValue / 13.62 - 36.7107);
   }
}

int __stdcall gpsgotposition(CPhidgetGPSHandle phid, void *usrPtr, int status){
if (status == 0){
printf("GPS position lost!\n");
} else if (status == 1){
printf("GPS position recived!\n");
}
}


int main(){
system("cls");

CPhidgetInterfaceKit_create(&ifkit);
CPhidgetMotorControl_create(&motor);
CPhidgetGPS_create(&gps);

CPhidget_openRemote((CPhidgetHandle)ifkit, -1, NULL, NULL);
CPhidget_openRemote((CPhidgetHandle)motor, -1, NULL, NULL);
CPhidget_openRemote((CPhidgetHandle)gps, -1, NULL, NULL);

CPhidget_set_OnAttach_Handler((CPhidgetHandle)ifkit, ifkitadded, NULL);
CPhidget_set_OnAttach_Handler((CPhidgetHandle)motor, motoradded, NULL);
CPhidget_set_OnAttach_Handler((CPhidgetHandle)gps, gpsadded, NULL);

CPhidget_set_OnDetach_Handler((CPhidgetHandle)ifkit, ifkitremoved, NULL);
CPhidget_set_OnDetach_Handler((CPhidgetHandle)motor, motorremoved, NULL);
CPhidget_set_OnDetach_Handler((CPhidgetHandle)gps, gpsremoved, NULL);

CPhidgetGPS_set_OnPositionChange_Handler(gps, gpsmoved, NULL);
CPhidgetGPS_set_OnPositionFixStatusChange_Handler(gps, gpsgotposition, NULL);
CPhidgetInterfaceKit_set_OnSensorChange_Handler(ifkit, ifkitchange, NULL);

CPhidget_waitForAttachment((CPhidgetHandle)ifkit, 10000);
CPhidget_waitForAttachment((CPhidgetHandle)motor, 10000);
CPhidget_waitForAttachment((CPhidgetHandle)gps, 10000);

bool ison;

while (1){
// Heartbeat
if (ison == false){
CPhidgetInterfaceKit_setOutputState(ifkit, 1, PFALSE);
ison = true;
} else if (ison == true){
CPhidgetInterfaceKit_setOutputState(ifkit, 1, PTRUE);
ison = false;
}

// Right Shift - Brake
if(GetKeyState(VK_RSHIFT) & 0x80){
CPhidgetMotorControl_setVelocity(motor, 0, 0);
CPhidgetMotorControl_setVelocity(motor, 1, 0);
CPhidgetInterfaceKit_setOutputState(ifkit, 6, PTRUE);
printf("Braking...\n");
} else {
CPhidgetInterfaceKit_setOutputState(ifkit, 6, PFALSE);


// Forward Back
if(GetKeyState(VK_UP) & 0x80){
CPhidgetMotorControl_setVelocity(motor, 0, 100);
} else if (GetKeyState(VK_DOWN) & 0x80){
CPhidgetMotorControl_setVelocity(motor, 0, -100);
} else {
CPhidgetMotorControl_setVelocity(motor, 0, 0);
}

// Left Right
if(GetKeyState(VK_LEFT) & 0x80){
CPhidgetMotorControl_setVelocity(motor, 1, 100);
} else if(GetKeyState(VK_RIGHT) & 0x80){
CPhidgetMotorControl_setVelocity(motor, 1, -100);
} else {
CPhidgetMotorControl_setVelocity(motor, 1, 0);
}
}


Sleep(100);
}

CPhidget_close((CPhidgetHandle)&ifkit);
CPhidget_delete((CPhidgetHandle)&ifkit);
CPhidget_close((CPhidgetHandle)&gps);
CPhidget_delete((CPhidgetHandle)&gps);
CPhidget_close((CPhidgetHandle)&motor);
CPhidget_delete((CPhidgetHandle)&motor);

return 0;
}


to compile place phidget21.h, phidget21.lib, and the .cpp file with the code in it in the same directory, cd into that directory in DOS, and to compile (rccar.zip has all of these files):

Code: Select all

c++ rccar.cpp -o rccar.exe phidget21.lib


If you have bonjor installed the code will work fine and connect when you run it, but without bonjor, some code modifications need to occur.

To use without bonjor, replace

Code: Select all

CPhidget_openRemote((CPhidgetHandle)ifkit, -1, NULL, NULL);
CPhidget_openRemote((CPhidgetHandle)motor, -1, NULL, NULL);
CPhidget_openRemote((CPhidgetHandle)gps, -1, NULL, NULL);

with

Code: Select all

CPhidget_openRemoteIP((CPhidgetHandle)ifkit, -1, "my.ip.address", 5001 ,NULL);
CPhidget_openRemoteIP((CPhidgetHandle)gps, -1, "my.ip.address", 5001 ,NULL);
CPhidget_openRemoteIP((CPhidgetHandle)motor, -1, "my.ip.address", 5001 ,NULL);


If the computer disconnects, the car will just keep on driving. To address this, in the computer code there is a section called by a comment as "heartbeat" this turns an output on and off every 100 milliseconds. The SBC runs some code that revives the heartbeat and if the heartbeat stops, it stops the motors. The code is

Code: Select all

#include <stdio.h>
#include <phidget21.h>

int AttachHandler(CPhidgetHandle IFK, void *userptr)
{
   int serialNo;
   const char *name;

   CPhidget_getDeviceName(IFK, &name);
   CPhidget_getSerialNumber(IFK, &serialNo);

   printf("%s %10d attached!\n", name, serialNo);

   return 0;
}

int DetachHandler(CPhidgetHandle IFK, void *userptr)
{
   int serialNo;
   const char *name;

   CPhidget_getDeviceName (IFK, &name);
   CPhidget_getSerialNumber(IFK, &serialNo);

   printf("%s %10d detached!\n", name, serialNo);

   return 0;
}

int ErrorHandler(CPhidgetHandle IFK, void *userptr, int ErrorCode, const char *unknown)
{
   printf("Error handled. %d - %s \n", ErrorCode, unknown);
   return 0;
}


int display_properties(CPhidgetInterfaceKitHandle phid)
{
   int serialNo, version, numInputs, numOutputs, numSensors, triggerVal, ratiometric;
   const char* ptr;

   CPhidget_getDeviceType((CPhidgetHandle)phid, &ptr);
   CPhidget_getSerialNumber((CPhidgetHandle)phid, &serialNo);
   CPhidget_getDeviceVersion((CPhidgetHandle)phid, &version);

   CPhidgetInterfaceKit_getInputCount(phid, &numInputs);
   CPhidgetInterfaceKit_getOutputCount(phid, &numOutputs);
   CPhidgetInterfaceKit_getSensorCount(phid, &numSensors);
   CPhidgetInterfaceKit_getRatiometric(phid, &ratiometric);

   printf("%s\n", ptr);
   printf("Serial Number: %10d\nVersion: %8d\n", serialNo, version);
   printf("# Digital Inputs: %d\n# Digital Outputs: %d\n", numInputs, numOutputs);
   printf("# Sensors: %d\n", numSensors);
   printf("Ratiometric: %d\n", ratiometric);

   return 0;
}


int interfacekit_simple()
{
   int result, numSensors, i;
   const char *err;

   //Declare an InterfaceKit handle
   CPhidgetInterfaceKitHandle ifKit = 0;
   CPhidgetMotorControlHandle motor = 0;
   //create the InterfaceKit object
   CPhidgetInterfaceKit_create(&ifKit);
   CPhidgetMotorControl_create(&motor);
   
   //Set the handlers to be run when the device is plugged in or opened from software, unplugged or closed from software, or generates an error.
   CPhidget_set_OnAttach_Handler((CPhidgetHandle)ifKit, AttachHandler, NULL);
   CPhidget_set_OnDetach_Handler((CPhidgetHandle)ifKit, DetachHandler, NULL);
   CPhidget_set_OnError_Handler((CPhidgetHandle)ifKit, ErrorHandler, NULL);

   //open the interfacekit for device connections
   CPhidget_openRemote((CPhidgetHandle)ifKit, -1, NULL, NULL);
   CPhidget_openRemote((CPhidgetHandle)motor, -1, NULL, NULL);
   //get the program to wait for an interface kit device to be attached
   printf("Waiting for interface kit to be attached.... \n");
   if((result = CPhidget_waitForAttachment((CPhidgetHandle)ifKit, 10000)))
   {
      CPhidget_getErrorDescription(result, &err);
      printf("Problem waiting for attachment: %s\n", err);
      return 0;
   }

   //Display the properties of the attached interface kit device
   display_properties(ifKit);

   int ison, wason, shouldbreak, failcount;
   while(1){

   CPhidgetInterfaceKit_getOutputState(ifKit, 7, &ison);
   CPhidgetInterfaceKit_getOutputState(ifKit, 6, &shouldbreak);

   
   if (shouldbreak == PTRUE){
   printf("Recived disconnect signal from iPhone \n");
   break;
   }
   
   if (ison == PTRUE && wason == 1){
   failcount++;
   if (failcount > 3){
   printf("stop!\n");
   CPhidgetMotorControl_setVelocity(motor, 0, 0);
   CPhidgetMotorControl_setVelocity(motor, 1, 0);
   }
   } else if (ison == PFALSE && wason == 0){
   failcount++;
   if (failcount > 3){
   CPhidgetMotorControl_setVelocity(motor, 0, 0);
   CPhidgetMotorControl_setVelocity(motor, 1, 0);
   printf("stop!\n");
   }
   } else {
   failcount = 0;
   }
   
   if (ison == PTRUE){
   wason = 1;
   } else if (ison == PFALSE){
   wason = 0;
   }
   
   usleep(100000);
   }
   
   CPhidget_close((CPhidgetHandle)ifKit);
   CPhidget_delete((CPhidgetHandle)ifKit);
   CPhidget_close((CPhidgetHandle)motor);
   CPhidget_delete((CPhidgetHandle)motor);

   return 0;
}

int main(int argc, char* argv[])
{
   interfacekit_simple();
   return 0;
}



If you have the C++ toolkit installed on the SBC, you can upload it, and SSH into the directory, and compile on the SBC. The command to compile on the SBC is

Code: Select all

gcc InterfaceKit-simple.cpp -o ifkit -lphidget21

to run it
./ifkit


and thats it!
Attachments
InterfaceKit-simple.c
To run on the SBC
(4.81 KiB) Downloaded 284 times
rccar.zip
Includes
rccar.cpp
rccar.exe
phidget21.lib
phidger21.h - modified for mingw
(66.25 KiB) Downloaded 333 times
phidget21.h
Modified for MinGW
(159.2 KiB) Downloaded 292 times
Last edited by Jacob on Sun Apr 24, 2011 3:28 pm, edited 2 times in total.

Return to “Robotics”

Who is online

Users browsing this forum: No registered users and 1 guest