int main(int argc, char *argv[]) {
int i, n;
char buff[128], ch;
if (gpioInitialise()<0) return 1;
gpioSetMode(ECHO, PI_INPUT);
// register callback on change of sonar pin
gpioSetAlertFunc(ECHO, range);
sleep(2);
while (1) {
ping(); // prime the last_range variable
sleep(1);
printf("%d \n", last_range);
}
sleep(1);
puts("Bye now!");
gpioTerminate();
return 0;
}
int main(int argc, char *argv[])
{
time_t starttime = time(NULL);
/* Can't instantiate lwrf classes before pigpio is initialised. */
if (gpioInitialise() >= 0)
{
std::string TX_TEST = "0001F15AC2";
LwrfRX rx(RX, RX_REPEAT);
LwrfTX tx(TX);
tx.Put(TX_TEST, TX_REPEAT);
do
{
if (rx.Ready() > 0)
{
std::cout << rx.Get() << std::endl;
}
else
{
//No messages, pause for 50msec breather
usleep(50000);
}
} while ((time(NULL) - starttime) < 30);
std::cout << "Rx debug " << rx.Debug() << std::endl;
std::cout << "Tx debug " << tx.Debug() << std::endl;
}
}
bool RPi2DOutSI::initialise(int pa_nPin, int pa_nPud) {
#ifdef POSIX
FZRTE_INFO("RPi2DOutSI: Initialising Gpio Pin %d to %d Mode",pa_nPin, pa_nPud);
int nRetVal = ZERO_VALUE;
nRetVal = gpioInitialise();
if(nRetVal < ZERO_VALUE){
FZRTE_ERROR("RPi2DOutSI : pigpio gpioInitialisation Failed : errorcode : %d",nRetVal);
return false;
}
nRetVal = gpioSetMode(pa_nPin, PI_OUTPUT);
if(nRetVal < ZERO_VALUE){
FZRTE_ERROR("RPi2DOutSI : pigpio SetMode Failed for Pin %d : errorcode : %d", pa_nPin,nRetVal);
return false;
}
switch(pa_nPud){// passing int pa_nPud doesn't work sometime
case 1:
nRetVal = gpioSetPullUpDown(pa_nPin, PI_PUD_DOWN);
break;
case 2:
nRetVal = gpioSetPullUpDown(pa_nPin, PI_PUD_OFF);
break;
default:
nRetVal = gpioSetPullUpDown(pa_nPin, PI_PUD_UP);
break;
}
if(nRetVal < ZERO_VALUE){
FZRTE_ERROR("RPi2DOutSI : pigpio SetPullUpDown Failed for Pin %d : errorcode : %d", pa_nPin, nRetVal);
return false;
}
#endif
return true;
}
int main( void ) {
gpioInitialise();
gpioSetAlertFunc( 4, callback );
sleep( 10 );
gpioTerminate();
return 0;
}
void console_print(void)
{
unsigned char x = 0;
if (gpioInitialise())
{
WriteLog("%s:%d - Init OK\n", __FILE__, __LINE__);
}
else WriteLog("%s:%d - Init NOT OK\n", __FILE__, __LINE__);
while (1)
{
if (zero_cros_diff_count != 0)
{
// if (zero_cros_diff_count == 1)
// zero_cros_diff_time_mean = 9990;
// else
zero_cros_diff_time_mean = zero_cros_diff_time/zero_cros_diff_count;
}
else
{
zero_cros_diff_time_mean = 9990;
}
printf("ac_present: %d; mean: %d; count: %d; pump_pwm: %d\n",ac_present, zero_cros_diff_time_mean, zero_cros_diff_count, pump1_pwm);
zero_cros_diff_time = zero_cros_diff_time_mean;
zero_cros_diff_count = 1;
// x++;
// if ((x % 5) == 0)
// {
// pump_pwm++;
// pump_pwm = pump_pwm % 10;
// }
sleep(5);
}
gpioTerminate();
pthread_exit(NULL);
}
int main(int argc, char *argv[])
{
int secs=60;
if (argc>1)
secs = atoi(argv[1]); /* program run seconds */
if ((secs<1) || (secs>3600))
secs = 3600;
if (gpioInitialise()<0)
return 1;
gpioSetMode(HALL, PI_INPUT);
gpioSetPullUpDown(HALL, PI_PUD_UP);
gpioSetAlertFunc(HALL, alert);
sleep(secs);
printf("%d\n", count);
gpioTerminate();
}
int main(void)
{
//initialise pigio and pins
if (gpioInitialise() < 0 )
{
printf("pigpio initialise failed\n");
return 1;
}
gpioSetMode(BLUE, PI_OUTPUT);
gpioSetMode(RED, PI_OUTPUT);
gpioSetMode(GREEN, PI_OUTPUT);
gpioSetPWMrange(BLUE, 200);
gpioSetPWMrange(RED, 200);
gpioSetPWMrange(GREEN, 200);
pthread_t pth; // this is our thread identifier
pthread_create(&pth,NULL,threadFunc,NULL);
int i;
while(1) {
if(MODE != 0) {
for (i=0; i<201; i++) {
if (MODE == 1) {
sLED(0,0,i,1);
usleep(20000);
}
else if(MODE == 2) {
sLED(i,i/2,i/4,1);
usleep(20000);
}
else if(MODE == 0) {
break;
}
}
sleep(1);
for(i=200; i>=0; i--) {
if(MODE == 1) {
sLED(0,0,i,1);
usleep(20000);
}
else if(MODE == 2) {
sLED(i,i/2,i/4,1);
usleep(20000);
}
else if(MODE == 0) {
break;
}
}
}
}
pthread_join(pth,NULL);
return 1;
}
/*
* Class: com_diozero_pigpioj_PigpioGpio
* Method: initialise
* Signature: ()I
*/
JNIEXPORT jint JNICALL Java_com_diozero_pigpioj_PigpioGpio_initialise
(JNIEnv* env, jclass clz) {
int i;
for (i = 0; i < MAX_GPIO_PINS; i++) {
listeners[i] = NULL;
}
return gpioInitialise();
}
void SetMTX2Frequency(char *FrequencyString)
{
float _mtx2comp;
int _mtx2int;
long _mtx2fractional;
char _mtx2command[17];
int fd;
double Frequency;
int i, pulses, wave_id;
char *str;
if (strlen(FrequencyString) < 3)
{
// Convert channel number to frequency
Frequency = strtol(FrequencyString, NULL, 16) * 0.003125 + 434.05;
printf("Channel %s\n", FrequencyString);
}
else
{
Frequency = atof(FrequencyString);
printf("Frequency %s\n", FrequencyString);
}
printf("MTX2 Frequency to be set to %8.4fMHz\n", Frequency);
_mtx2comp=(Frequency+0.0015)/6.5;
_mtx2int=_mtx2comp;
_mtx2fractional = ((_mtx2comp-_mtx2int)+1) * 524288;
snprintf(_mtx2command,17,"@PRG_%02X%06lX\r",_mtx2int-1, _mtx2fractional);
printf("MTX2 command is %s\n", _mtx2command);
if (gpioInitialise() < 0)
{
printf("pigpio initialisation failed.\n");
return;
}
gpioSetMode(NTX2B_ENABLE_BCM, PI_OUTPUT);
gpioWaveAddNew();
gpioWaveAddSerial(NTX2B_ENABLE_BCM, 9600, 8, 2, 0, strlen(_mtx2command), _mtx2command);
wave_id = gpioWaveCreate();
if (wave_id >= 0)
{
gpioWaveTxSend(wave_id, 0);
while (gpioWaveTxBusy())
{
time_sleep(0.1);
}
}
gpioTerminate();
}
int main(int argc, char* argv[])
{
gpioInitialise();
int spiHandle = i2cOpen(1, 0x26, 0);
i2cWriteByte(spiHandle, 3);
gpioDelay(100);
i2cClose(spiHandle);
return 0;
}
//#######################################MAIN#######################################
int main(int argc, char** argv) {
signal(SIGINT, stopHandler); /* catches ctrl-c */
if (gpioInitialise() < 0)
{
fprintf(stderr, "pigpio initialisation failed\n");
return 1;
}
gpioSetMode(17, PI_OUTPUT);
gpioSetMode(22, PI_INPUT);
gpioSetMode(24, PI_OUTPUT);
UA_ServerConfig config = UA_ServerConfig_standard;
UA_ServerNetworkLayer nl = UA_ServerNetworkLayerTCP(UA_ConnectionConfig_standard, 16664);
config.networkLayers = &nl;
config.networkLayersSize = 1;
UA_Server *server = UA_Server_new(config);
/* add a variable node to the address space */
UA_Int32 myIntegerB = 32;
UA_NodeId myIntegerBNodeId = UA_NODEID_STRING(1, "button");
UA_QualifiedName myIntegerBName = UA_QUALIFIEDNAME(1, "button");
UA_DataSource dateDataSource = (UA_DataSource) {
.handle = &myIntegerB, .read = readIntegerB, .write = writeIntegerB};
UA_VariableAttributes attr;
UA_VariableAttributes_init(&attr);
attr.description = UA_LOCALIZEDTEXT("en_US","button_on_off");
attr.displayName = UA_LOCALIZEDTEXT("en_US","button");
UA_Server_addDataSourceVariableNode(server, myIntegerBNodeId,
UA_NODEID_NUMERIC(0, UA_NS0ID_OBJECTSFOLDER),
UA_NODEID_NUMERIC(0, UA_NS0ID_ORGANIZES),
myIntegerBName, UA_NODEID_NULL, attr, dateDataSource, NULL);
/* add a repeated job to the server */
UA_Job job = {.type = UA_JOBTYPE_METHODCALL,
.job.methodCall = {.method = testCallback, .data = NULL} };
UA_Server_addRepeatedJob(server, job, 2000, NULL); // call every 2 sec
UA_StatusCode retval = UA_Server_run(server, &running);
UA_Server_delete(server);
nl.deleteMembers(&nl);
return (int)retval;
}
int main(int argc, char *argv[])
{
if (gpioInitialise() < 0)
return 1;
//TODO: Pull low these GPIO pins on startup!
// SDA0 GPIO 0
gpioSetMode(17, PI_OUTPUT);
gpioWrite(17, PI_LOW);
// SCL0 GPIO 1
gpioSetMode(18, PI_OUTPUT);
gpioWrite(18, PI_LOW);
// SDA1 GPIO 2
gpioSetMode(27, PI_OUTPUT);
gpioWrite(27, PI_LOW);
// SCL1 GPIO 3
gpioSetMode(22, PI_OUTPUT);
gpioWrite(22, PI_LOW);
// GPIO 4
gpioSetMode(23, PI_OUTPUT);
gpioWrite(23, PI_LOW);
// GPIO 5
gpioSetMode(24, PI_OUTPUT);
gpioWrite(24, PI_LOW);
// GPIO 6
gpioSetMode(25, PI_OUTPUT);
gpioWrite(25, PI_LOW);
// GPIO 7
gpioSetMode(4, PI_OUTPUT);
gpioWrite(4, PI_LOW);
// GPIO 8
gpioSetMode(2, PI_OUTPUT);
gpioWrite(2, PI_LOW);
// GPIO 9
gpioSetMode(3, PI_OUTPUT);
gpioWrite(3, PI_LOW);
// // GPIO 34
// gpioSetMode(4, PI_OUTPUT);
// gpioWrite(4, PI_LOW);
// // GPIO 35
// gpioSetMode(4, PI_OUTPUT);
// gpioWrite(4, PI_LOW);
// // GPIO 36
// gpioSetMode(4, PI_OUTPUT);
// gpioWrite(4, PI_LOW);
QApplication a(argc, argv);
MainWindow w;
w.show();
return a.exec();
}
int main(int argc, char *argv[])
{
Pi_Renc_t * renc;
if (gpioInitialise() < 0) return 1;
renc = Pi_Renc(7, 8, callback);
sleep(300);
Pi_Renc_cancel(renc);
gpioTerminate();
}
void initialiseSonars()
{
int status;
status = gpioInitialise();
if (status < 0)
{
fprintf(stderr, "Sonar pigpio initialisation failed.\n");
}
int i = 0;
for (i=0;i<4;i++)
{
gpioSetMode(Echos[i], PI_INPUT);
gpioSetMode(Trigs[i], PI_OUTPUT);
}
}
int main(void) {
int rc = gpioInitialise();
if (rc < 0) {
printf("gpioInitialise() failed: %d\n", rc);
exit(-1);
}
int pwmPin = 17;
int servoSweepDuration = 500; // Time in ms
printf("pwmPin pin = %d\n", pwmPin);
printf("servoSweepDuration = %d seconds\n", servoSweepDuration);
int direction = 0; // 0 = up, 1 = down
while(1) {
unsigned int startTime = gpioTick();
unsigned int sweepTime = servoSweepDuration * 1000 / 2;
unsigned int delta = gpioTick() - startTime;
while(delta < sweepTime) {
// Position is going to be between 0 and 1 and represents how far along the sweep
// we are.
double position = (double)delta/sweepTime;
unsigned int timeToWriteHighUSecs;
position = -0.4;
if (direction == 0) {
timeToWriteHighUSecs = (position + 1.0) * 1000;
} else {
timeToWriteHighUSecs = (2.0 - position) * 1000;
}
printf("Setting pulse width %d, %d, %d\n", timeToWriteHighUSecs, delta, startTime);
gpioServo(pwmPin, timeToWriteHighUSecs);
gpioDelay(20 * 1000);
delta = gpioTick() - startTime;
} // End of a sweep in one direction
// Switch direction
if (direction == 0) {
direction = 1;
} else {
direction = 0;
}
} // End of while true
return 0;
}
int main(int argc, char *argv[])
{
int loops=LOOPS;
int speed=SPEED;
int bytes=BYTES;
int i;
int h;
double start, diff, sps;
unsigned char buf[16384];
if (argc > 1) bytes = atoi(argv[1]);
if ((bytes < 1) || (bytes > 16383)) bytes = BYTES;
if (argc > 2) speed = atoi(argv[2]);
if ((speed < 32000) || (speed > 32000000)) speed = SPEED;
if (argc > 3) loops = atoi(argv[3]);
if ((loops < 1) || (loops > 10000000)) loops = LOOPS;
if (gpioInitialise() < 0) return 1;
h = spiOpen(0, speed, 0);
if (h < 0) return 2;
start = time_time();
for (i=0; i<loops; i++)
{
spiXfer(h, buf, buf, bytes);
}
diff = time_time() - start;
printf("sps=%.1f: %d bytes @ %d bps (loops=%d time=%.1f)\n",
(double)loops / diff, bytes, speed, loops, diff);
spiClose(h);
gpioTerminate();
return 0;
}
int main() {
if(gpioInitialise() < 0) return 1; // init pigpio lib
if(gpioSetMode(DUST_PIN, PI_INPUT) < 0) return 1; // configure DUST_PIN as input
if(gpioSetAlertFunc(DUST_PIN, edge_detected) < 0) return 1; // configure interrupt routine for DUST_PIN
if(gpioDelay(SAMPLETIME_MS * 1000) < 0) return 1; // wait SAMPLETIME_MS (gpioDelay wants us)
gpioTerminate(); // try to terminate pigpio lib
float ratio = (lowOccupancy / SAMPLETIME_MS) * 100; // calculate low occupancy in percent
/*
calculate particle concentration
-> amount of particles per 283ml | particle size > 1um
eqation for calculation from http://www.howmuchsnow.com/arduino/airquality/grovedust/
*/
float concentration = 1.1 * pow(ratio, 3) - 3.8 * pow(ratio, 2) + 520 * ratio + 0.62;
printf("%d %f %f", lowOccupancy, ratio, concentration);
return 0;
}
int main() {
int ythrust;
gpioInitialise();
mygpioSetup();
printf("gpio setup");
while (1) {
ythrust = 2000;
gpioPWM(18, ythrust);
printf("gpio 18 at 2000");
sleep(5);
ythrust = 1500;
gpioPWM(18, ythrust);
printf("gpio 18 at 1500");
sleep(5);
ythrust = 1000;
gpioPWM(18, ythrust);
printf("gpio 18 at 1000");
sleep(5);
}
gpioTerminate();
return 0;
}
/*
* Updates function
*
*/
void motorControlFCN_Update_wrapper(const real32_T *motorSpeed,
real_T *xD)
{
/* %%%-SFUNWIZ_wrapper_Update_Changes_BEGIN --- EDIT HERE TO _END */
if(xD[0] != 1){
# ifndef MATLAB_MEX_FILE
if (gpioInitialise() < 0)
{
printf("Init Failed\n");
}
else
{
printf("Init Successful");
}
#endif
//done with initialization
xD[0] = 1;
}
/* %%%-SFUNWIZ_wrapper_Update_Changes_END --- EDIT HERE TO _BEGIN */
}
/**
* Initialises the pigpio library and sensor array ready for use
*/
void setup(void) {
// Initialise the pigpio library
gpioInitialise();
// Configure the LED_ON pin as an output, and set to 'low'
gpioSetMode(LED_ON, PI_OUTPUT);
gpioWrite(LED_ON, 0);
// We need to monitor changes on multiple GPIO pins and the time their value
// change. So we're going to use the 'notify' pipe to do this
notifyPipe = gpioNotifyOpen();
if(notifyPipe < 0) {
std::cerr << "Failed to open notification pipe! Exitting." << std::endl;
exit(-1);
}
// Generate the path to the notify pipe and open it
std::ostringstream notifyPathStream;
notifyPathStream << "/dev/pigpio" << notifyPipe;
notifyPipeFD = open(notifyPathStream.str().c_str(), O_RDONLY|O_NONBLOCK);
if(notifyPipeFD < 0) {
std::cerr << "Failed to open notify path [" << notifyPathStream.str() << "]" << std::endl;
exit(-1);
}
// We want to monitor all 8 GPIOs
notificationGPIOs = 0;
for(size_t i = 0; i < 8; i++) {
notificationGPIOs |= (1 << sensorGPIOs[i]);
}
// Disable any pull up resistors
// Is this needed?
for(size_t i = 0; i < 8; i++) {
//gpioSetPullUpDown(sensorGPIOs[i], PI_PUD_DOWN);
}
}
int init_hardware()
{
if (gpioInitialise() < 0)
{
printf("GPIO initialization failed \n");
return -1;
}
//gpioSetMode(24,PI_OUTPUT);
// set motor pins
gpioSetMode(12,PI_OUTPUT);
gpioSetMode(16,PI_OUTPUT);
gpioSetMode(20,PI_OUTPUT);
gpioSetMode(21,PI_OUTPUT);
//how many detail levels (1 = just the capture res, > 1 goes down by half each level, 4 max)
//int num_levels = 1;
cam = StartCamera(CAMERA_WIDTH, CAMERA_HEIGHT,30,1,true);
if (cam == NULL)
{
printf(" Camera initialization failure\n");
return -2;
}
// screen parameters
spi_h = spiOpen(0,5000,0);
if (spi_h<0)
{
printf("SPI failure\n");
return -3;
}
// success
// give camera time to settle
sleep(1);
return 0;
}
UdpClient::UdpClient()
{
m_pJsonmodel = new PerceptronJsonModel("");
m_UdpSocket.bind(QHostAddress("192.168.2.167"), 7777);
QObject::connect(&m_UdpSocket,SIGNAL(readyRead()),this,SLOT(sltReadDatagram()));
if (gpioInitialise() < 0)
{
fprintf(stderr, "pigpio initialisation failed\n");
}
else
{
/* Set GPIO modes */
gpioSetMode(2, PI_OUTPUT);
gpioSetMode(3, PI_OUTPUT);
gpioSetMode(4, PI_OUTPUT);
gpioSetMode(17, PI_OUTPUT);
}
}
int initialize(){
// Call this necessary function
if (gpioInitialise() < 0)
{
// pigpio initialisation failed.
return -1;
}
// Set all GPIO pins to be used as outputs
gpioSetMode(LIGHT1_ON, PI_OUTPUT);
gpioSetMode(LIGHT2_ON, PI_OUTPUT);
gpioSetMode(LIGHT3_ON, PI_OUTPUT);
gpioSetMode(LIGHT4_ON, PI_OUTPUT);
gpioSetMode(LIGHT5_ON, PI_OUTPUT);
gpioSetMode(LIGHT1_OFF, PI_OUTPUT);
gpioSetMode(LIGHT2_OFF, PI_OUTPUT);
gpioSetMode(LIGHT3_OFF, PI_OUTPUT);
gpioSetMode(LIGHT4_OFF, PI_OUTPUT);
gpioSetMode(LIGHT5_OFF, PI_OUTPUT);
return 0;
}
int main(int argc, char* argv[]) {
CvMemStorage *contStorage = cvCreateMemStorage(0);
CvSeq *contours;
CvTreeNodeIterator polyIterator;
int found = 0;
int i;
CvPoint poly_point;
int fps=30;
// ポリライン近似
CvMemStorage *polyStorage = cvCreateMemStorage(0);
CvSeq *polys, *poly;
// OpenCV variables
CvFont font;
printf("start!\n");
//pwm initialize
if(gpioInitialise() < 0) return -1;
//pigpio CW/CCW pin setup
//X:18, Y1:14, Y2:15
gpioSetMode(18, PI_OUTPUT);
gpioSetMode(14, PI_OUTPUT);
gpioSetMode(15, PI_OUTPUT);
//pigpio pulse setup
//X:25, Y1:23, Y2:24
gpioSetMode(25, PI_OUTPUT);
gpioSetMode(23, PI_OUTPUT);
gpioSetMode(24, PI_OUTPUT);
//limit-switch setup
gpioSetMode(5, PI_INPUT);
gpioWrite(5, 0);
gpioSetMode(6, PI_INPUT);
gpioWrite(6, 0);
gpioSetMode(7, PI_INPUT);
gpioWrite(7, 0);
gpioSetMode(8, PI_INPUT);
gpioWrite(8, 0);
gpioSetMode(13, PI_INPUT);
gpioSetMode(19, PI_INPUT);
gpioSetMode(26, PI_INPUT);
gpioSetMode(21, PI_INPUT);
CvCapture* capture_robot_side = cvCaptureFromCAM(0);
CvCapture* capture_human_side = cvCaptureFromCAM(1);
if(capture_robot_side == NULL){
std::cout << "Robot Side Camera Capture FAILED" << std::endl;
return -1;
}
if(capture_human_side ==NULL){
std::cout << "Human Side Camera Capture FAILED" << std::endl;
return -1;
}
// size設定
cvSetCaptureProperty(capture_robot_side,CV_CAP_PROP_FRAME_WIDTH,CAM_PIX_WIDTH);
cvSetCaptureProperty(capture_robot_side,CV_CAP_PROP_FRAME_HEIGHT,CAM_PIX_HEIGHT);
cvSetCaptureProperty(capture_human_side,CV_CAP_PROP_FRAME_WIDTH,CAM_PIX_WIDTH);
cvSetCaptureProperty(capture_human_side,CV_CAP_PROP_FRAME_HEIGHT,CAM_PIX_HEIGHT);
//fps設定
cvSetCaptureProperty(capture_robot_side,CV_CAP_PROP_FPS,fps);
cvSetCaptureProperty(capture_human_side,CV_CAP_PROP_FPS,fps);
// 画像の表示用ウィンドウ生成
//cvNamedWindow("Previous Image", CV_WINDOW_AUTOSIZE);
// cvNamedWindow("Now Image", CV_WINDOW_AUTOSIZE);
// cvNamedWindow("pack", CV_WINDOW_AUTOSIZE);
// cvNamedWindow("mallet", CV_WINDOW_AUTOSIZE);
// cvNamedWindow ("Poly", CV_WINDOW_AUTOSIZE);
//Create trackbar to change brightness
int iSliderValue1 = 50;
cvCreateTrackbar("Brightness", "Now Image", &iSliderValue1, 100);
//Create trackbar to change contrast
int iSliderValue2 = 50;
cvCreateTrackbar("Contrast", "Now Image", &iSliderValue2, 100);
//pack threthold 0, 50, 120, 220, 100, 220
int iSliderValuePack1 = 54; //80;
cvCreateTrackbar("minH", "pack", &iSliderValuePack1, 255);
int iSliderValuePack2 = 84;//106;
cvCreateTrackbar("maxH", "pack", &iSliderValuePack2, 255);
int iSliderValuePack3 = 100;//219;
cvCreateTrackbar("minS", "pack", &iSliderValuePack3, 255);
int iSliderValuePack4 = 255;//175;
cvCreateTrackbar("maxS", "pack", &iSliderValuePack4, 255);
int iSliderValuePack5 = 0;//29;
cvCreateTrackbar("minV", "pack", &iSliderValuePack5, 255);
int iSliderValuePack6 = 255;//203;
cvCreateTrackbar("maxV", "pack", &iSliderValuePack6, 255);
//mallet threthold 0, 255, 100, 255, 140, 200
int iSliderValuemallet1 = 106;
cvCreateTrackbar("minH", "mallet", &iSliderValuemallet1, 255);
int iSliderValuemallet2 = 135;
cvCreateTrackbar("maxH", "mallet", &iSliderValuemallet2, 255);
int iSliderValuemallet3 = 218;//140
cvCreateTrackbar("minS", "mallet", &iSliderValuemallet3, 255);
int iSliderValuemallet4 = 255;
cvCreateTrackbar("maxS", "mallet", &iSliderValuemallet4, 255);
int iSliderValuemallet5 = 0;
cvCreateTrackbar("minV", "mallet", &iSliderValuemallet5, 255);
int iSliderValuemallet6 = 105;
cvCreateTrackbar("maxV", "mallet", &iSliderValuemallet6, 255);
// 画像ファイルポインタの宣言
IplImage* img_robot_side = cvQueryFrame(capture_robot_side);
IplImage* img_human_side = cvQueryFrame(capture_human_side);
IplImage* img_all_round = cvCreateImage(cvSize(CAM_PIX_WIDTH, CAM_PIX_2HEIGHT), IPL_DEPTH_8U, 3);
IplImage* tracking_img = cvCreateImage(cvGetSize(img_all_round), IPL_DEPTH_8U, 3);
IplImage* img_all_round2 = cvCreateImage(cvGetSize(img_all_round), IPL_DEPTH_8U, 3);
IplImage* show_img = cvCreateImage(cvGetSize(img_all_round), IPL_DEPTH_8U, 3);
cv::Mat mat_frame1;
cv::Mat mat_frame2;
cv::Mat dst_img_v;
cv::Mat dst_bright_cont;
int iBrightness = iSliderValue1 - 50;
double dContrast = iSliderValue2 / 50.0;
IplImage* dst_img_frame = cvCreateImage(cvGetSize(img_all_round), IPL_DEPTH_8U, 3);
IplImage* grayscale_img = cvCreateImage(cvGetSize(img_all_round), IPL_DEPTH_8U, 1);
IplImage* poly_tmp = cvCreateImage( cvGetSize( img_all_round), IPL_DEPTH_8U, 1);
IplImage* poly_dst = cvCreateImage( cvGetSize( img_all_round), IPL_DEPTH_8U, 3);
IplImage* poly_gray = cvCreateImage( cvGetSize(img_all_round),IPL_DEPTH_8U,1);
int rotate_times = 0;
//IplImage* -> Mat
mat_frame1 = cv::cvarrToMat(img_robot_side);
mat_frame2 = cv::cvarrToMat(img_human_side);
//上下左右を反転。本番環境では、mat_frame1を反転させる
cv::flip(mat_frame1, mat_frame1, 0); //水平軸で反転（垂直反転）
cv::flip(mat_frame1, mat_frame1, 1); //垂直軸で反転（水平反転）
vconcat(mat_frame2, mat_frame1, dst_img_v);
dst_img_v.convertTo(dst_bright_cont, -1, dContrast, iBrightness); //１枚にした画像をコンバート
//画像の膨張と縮小
// cv::Mat close_img;
// cv::Mat element(3,3,CV_8U, cv::Scalar::all(255));
// cv::morphologyEx(dst_img_v, close_img, cv::MORPH_CLOSE, element, cv::Point(-1,-1), 3);
// cv::imshow("morphologyEx", dst_img_v);
// dst_img_v.convertTo(dst_bright_cont, -1, dContrast, iBrightness); //１枚にした画像をコンバート
//明るさ調整した結果を変換(Mat->IplImage*)して渡す。その後解放。
*img_all_round = dst_bright_cont;
cv_ColorExtraction(img_all_round, dst_img_frame, CV_BGR2HSV, 0, 54, 77, 255, 0, 255);
cvCvtColor(dst_img_frame, grayscale_img, CV_BGR2GRAY);
cv_Labelling(grayscale_img, tracking_img);
cvCvtColor(tracking_img, poly_gray, CV_BGR2GRAY);
cvCopy( poly_gray, poly_tmp);
cvCvtColor( poly_gray, poly_dst, CV_GRAY2BGR);
//画像の膨張と縮小
//cvMorphologyEx(tracking_img, tracking_img,)
// 輪郭抽出
found = cvFindContours( poly_tmp, contStorage, &contours, sizeof( CvContour), CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE);
// ポリライン近似
polys = cvApproxPoly( contours, sizeof( CvContour), polyStorage, CV_POLY_APPROX_DP, 8, 10);
cvInitTreeNodeIterator( &polyIterator, ( void*)polys, 10);
poly = (CvSeq *)cvNextTreeNode( &polyIterator);
printf("sort before by X\n");
for( i=0; i<poly->total; i++)
{
poly_point = *( CvPoint*)cvGetSeqElem( poly, i);
// cvCircle( poly_dst, poly_point, 1, CV_RGB(255, 0 , 255), -1);
// cvCircle( poly_dst, poly_point, 8, CV_RGB(255, 0 , 255));
std::cout << "x:" << poly_point.x << ", y:" << poly_point.y << std::endl;
}
printf("Poly FindTotal:%d\n",poly->total);
//枠の座標決定
//左上 の 壁サイド側 upper_left_f
//左上 の ゴール寄り upper_left_g
//右上 の 壁サイド側 upper_right_f
//右上 の ゴール寄り upper_right_g
//左下 の 壁サイド側 lower_left_f
//左下 の ゴール寄り lower_left_g
//右下 の 壁サイド側 lower_right_f
//右下 の ゴール寄り lower_right_g
CvPoint upper_left_f, upper_left_g, upper_right_f, upper_right_g,
lower_left_f, lower_left_g, lower_right_f, lower_right_g,
robot_goal_left, robot_goal_right;
CvPoint frame_points[8];
// if(poly->total == 8){
// for( i=0; i<8; i++){
// poly_point = *( CvPoint*)cvGetSeqElem( poly, i);
// frame_points[i] = poly_point;
// }
// qsort(frame_points, 8, sizeof(CvPoint), compare_cvpoint);
// printf("sort after by X\n");
// for( i=0; i<8; i++){
// std::cout << "x:" << frame_points[i].x << ", y:" << frame_points[i].y << std::endl;
// }
// if(frame_points[0].y < frame_points[1].y){
// upper_left_f = frame_points[0];
// lower_left_f = frame_points[1];
// }
// else{
// upper_left_f = frame_points[1];
// lower_left_f = frame_points[0];
// }
// if(frame_points[2].y < frame_points[3].y){
// upper_left_g = frame_points[2];
// lower_left_g = frame_points[3];
// }
// else{
// upper_left_g = frame_points[3];
// lower_left_g = frame_points[2];
// }
// if(frame_points[4].y < frame_points[5].y){
// upper_right_g = frame_points[4];
// lower_right_g = frame_points[5];
// }
// else{
// upper_right_g = frame_points[5];
// lower_right_g = frame_points[4];
// }
// if(frame_points[6].y < frame_points[7].y){
// upper_right_f = frame_points[6];
// lower_right_f = frame_points[7];
// }
// else{
// upper_right_f = frame_points[7];
// lower_right_f = frame_points[6];
// }
// }
// else{
printf("Frame is not 8 Point\n");
upper_left_f = cvPoint(26, 29);
upper_right_f = cvPoint(136, 29);
lower_left_f = cvPoint(26, 220);
lower_right_f = cvPoint(136, 220);
upper_left_g = cvPoint(38, 22);
upper_right_g = cvPoint(125, 22);
lower_left_g = cvPoint(38, 226);
lower_right_g = cvPoint(125, 226);
robot_goal_left = cvPoint(60, 226);
robot_goal_right = cvPoint(93, 226);
// cvCopy(img_all_round, show_img);
// cvLine(show_img, upper_left_f, upper_right_f, CV_RGB( 255, 255, 0 ));
// cvLine(show_img, lower_left_f, lower_right_f, CV_RGB( 255, 255, 0 ));
// cvLine(show_img, upper_right_f, lower_right_f, CV_RGB( 255, 255, 0 ));
// cvLine(show_img, upper_left_f, lower_left_f, CV_RGB( 255, 255, 0 ));
//
// cvLine(show_img, upper_left_g, upper_right_g, CV_RGB( 0, 255, 0 ));
// cvLine(show_img, lower_left_g, lower_right_g, CV_RGB( 0, 255, 0 ));
// cvLine(show_img, upper_right_g, lower_right_g, CV_RGB( 0, 255, 0 ));
// cvLine(show_img, upper_left_g, lower_left_g, CV_RGB( 0, 255, 0 ));
//while(1){
//cvShowImage("Now Image", show_img);
//cvShowImage ("Poly", poly_dst);
//if(cv::waitKey(1) >= 0) {
//break;
//}
//}
//return -1;
// }
printf("upper_left_fX:%d, Y:%d\n",upper_left_f.x, upper_left_f.y);
printf("upper_left_gX:%d, Y:%d\n",upper_left_g.x, upper_left_g.y);
printf("upper_right_fX:%d,Y:%d\n", upper_right_f.x, upper_right_f.y);
printf("upper_right_gX:%d, Y:%d\n" , upper_right_g.x, upper_right_g.y);
printf("lower_left_fX:%d, Y:%d\n", lower_left_f.x, lower_left_f.y);
printf("lower_left_gX:%d, Y:%d\n", lower_left_g.x, lower_left_g.y);
printf("lower_right_fX:%d, Y:%d\n", lower_right_f.x, lower_right_f.y);
printf("lower_right_gX:%d, Y:%d\n", lower_right_g.x, lower_right_g.y);
printf("robot_goal_left:%d, Y:%d\n", robot_goal_left.x, robot_goal_left.y);
printf("robot_goal_right:%d, Y:%d\n", robot_goal_right.x, robot_goal_right.y);
cvReleaseImage(&dst_img_frame);
cvReleaseImage(&grayscale_img);
cvReleaseImage(&poly_tmp);
cvReleaseImage(&poly_gray);
cvReleaseMemStorage(&contStorage);
cvReleaseMemStorage(&polyStorage);
//return 1;
// Init font
cvInitFont(&font,CV_FONT_HERSHEY_SIMPLEX|CV_FONT_ITALIC, 0.4,0.4,0,1);
bool is_pushed_decision_button = 1;//もう一方のラズパイ信号にする
while(1){
//決定ボタンが押されたらスタート
if(gpioRead(8)==0 && is_pushed_decision_button==1){
cvCopy(img_all_round, img_all_round2);
cvCopy(img_all_round, show_img);
img_robot_side = cvQueryFrame(capture_robot_side);
img_human_side = cvQueryFrame(capture_human_side);
//IplImage* -> Mat
mat_frame1 = cv::cvarrToMat(img_robot_side);
mat_frame2 = cv::cvarrToMat(img_human_side);
//上下左右を反転。本番環境では、mat_frame1を反転させる
cv::flip(mat_frame1, mat_frame1, 0); //水平軸で反転（垂直反転）
cv::flip(mat_frame1, mat_frame1, 1); //垂直軸で反転（水平反転）
vconcat(mat_frame2, mat_frame1, dst_img_v);
iBrightness = iSliderValue1 - 50;
dContrast = iSliderValue2 / 50.0;
dst_img_v.convertTo(dst_bright_cont, -1, dContrast, iBrightness); //１枚にした画像をコンバート
//明るさ調整した結果を変換(Mat->IplImage*)して渡す。その後解放。
*img_all_round = dst_bright_cont;
mat_frame1.release();
mat_frame2.release();
dst_img_v.release();
IplImage* dst_img_mallet = cvCreateImage(cvGetSize(img_all_round), IPL_DEPTH_8U, 3);
IplImage* dst_img_pack = cvCreateImage(cvGetSize(img_all_round), IPL_DEPTH_8U, 3);
IplImage* dst_img2_mallet = cvCreateImage(cvGetSize(img_all_round2), IPL_DEPTH_8U, 3);
IplImage* dst_img2_pack = cvCreateImage(cvGetSize(img_all_round2), IPL_DEPTH_8U, 3);
cv_ColorExtraction(img_all_round, dst_img_pack, CV_BGR2HSV, iSliderValuePack1, iSliderValuePack2, iSliderValuePack3, iSliderValuePack4, iSliderValuePack5, iSliderValuePack6);
cv_ColorExtraction(img_all_round, dst_img_mallet, CV_BGR2HSV, iSliderValuemallet1, iSliderValuemallet2, iSliderValuemallet3, iSliderValuemallet4, iSliderValuemallet5, iSliderValuemallet6);
cv_ColorExtraction(img_all_round2, dst_img2_pack, CV_BGR2HSV, iSliderValuePack1, iSliderValuePack2, iSliderValuePack3, iSliderValuePack4, iSliderValuePack5, iSliderValuePack6);
//CvMoments moment_mallet;
CvMoments moment_pack;
CvMoments moment_mallet;
CvMoments moment2_pack;
//cvSetImageCOI(dst_img_mallet, 1);
cvSetImageCOI(dst_img_pack, 1);
cvSetImageCOI(dst_img_mallet, 1);
cvSetImageCOI(dst_img2_pack, 1);
//cvMoments(dst_img_mallet, &moment_mallet, 0);
cvMoments(dst_img_pack, &moment_pack, 0);
cvMoments(dst_img_mallet, &moment_mallet, 0);
cvMoments(dst_img2_pack, &moment2_pack, 0);
//座標計算
double m00_before = cvGetSpatialMoment(&moment2_pack, 0, 0);
double m10_before = cvGetSpatialMoment(&moment2_pack, 1, 0);
double m01_before = cvGetSpatialMoment(&moment2_pack, 0, 1);
double m00_after = cvGetSpatialMoment(&moment_pack, 0, 0);
double m10_after = cvGetSpatialMoment(&moment_pack, 1, 0);
double m01_after = cvGetSpatialMoment(&moment_pack, 0, 1);
double gX_before = m10_before/m00_before;
double gY_before = m01_before/m00_before;
double gX_after = m10_after/m00_after;
double gY_after = m01_after/m00_after;
double m00_mallet = cvGetSpatialMoment(&moment_mallet, 0, 0);
double m10_mallet = cvGetSpatialMoment(&moment_mallet, 1, 0);
double m01_mallet = cvGetSpatialMoment(&moment_mallet, 0, 1);
double gX_now_mallet = m10_mallet/m00_mallet;
double gY_now_mallet = m01_mallet/m00_mallet;
int target_direction = -1; //目標とする向き 時計回り＝1、 反時計回り＝0
//円の大きさは全体の1/10で描画
// cvCircle(show_img, cvPoint(gX_before, gY_before), CAM_PIX_HEIGHT/10, CV_RGB(0,0,255), 6, 8, 0);
// cvCircle(show_img, cvPoint(gX_now_mallet, gY_now_mallet), CAM_PIX_HEIGHT/10, CV_RGB(0,0,255), 6, 8, 0);
// cvLine(show_img, cvPoint(gX_before, gY_before), cvPoint(gX_after, gY_after), cvScalar(0,255,0), 2);
// cvLine(show_img, robot_goal_left, robot_goal_right, cvScalar(0,255,255), 2);
printf("gX_after: %f\n",gX_after);
printf("gY_after: %f\n",gY_after);
printf("gX_before: %f\n",gX_before);
printf("gY_before: %f\n",gY_before);
printf("gX_now_mallet: %f\n",gX_now_mallet);
printf("gY_now_mallet: %f\n",gY_now_mallet);
int target_destanceY = CAM_PIX_2HEIGHT - 30; //Y座標の距離を一定にしている。ディフェンスライン。
//パックの移動は直線のため、一次関数の計算を使って、その後の軌跡を予測する。
double a_inclination;
double b_intercept;
int closest_frequency;
int target_coordinateX;
int origin_coordinateY;
int target_coordinateY;
double center_line = (lower_right_f.x + lower_right_g.x + lower_left_f.x + lower_left_g.x)/4;
int left_frame = (upper_left_f.x + lower_left_f.x)/2;
int right_frame = (upper_right_f.x + lower_right_f.x)/2;
if(robot_goal_right.x < gX_now_mallet){
gpioPWM(25, 128);
closest_frequency = gpioSetPWMfrequency(25, 1500);
target_direction = 0;//反時計回り
}
else if(gX_now_mallet < robot_goal_left.x){
gpioPWM(25, 128);
closest_frequency = gpioSetPWMfrequency(25, 1500);
target_direction = 1;//時計回り
}
else{
//pwm output for rotate
//台の揺れを想定してマージンをとる
if(abs(gX_after - gX_before) <= 1 && abs(gY_after - gY_before) <= 1){//パックが動いてない場合一時停止
gpioPWM(25, 0);
closest_frequency = gpioSetPWMfrequency(25, 0);
a_inclination = 0;
b_intercept=0;
}
else{
a_inclination = (gY_after - gY_before) / (gX_after - gX_before);
b_intercept = gY_after - a_inclination * gX_after;
//一次関数で目標X座標の計算
if(a_inclination){
target_coordinateX = (int)((target_destanceY - b_intercept) / a_inclination);
}
else{
target_coordinateX = center_line;
}
origin_coordinateY = a_inclination * left_frame + b_intercept;
int rebound_max = 5;
int rebound_num = 0;
while(target_coordinateX < left_frame || right_frame < target_coordinateX){
if(target_coordinateX < left_frame){ //左側の枠での跳ね返り後の軌跡。左枠側平均
target_coordinateX = 2 * left_frame - target_coordinateX;
b_intercept -= 2 * ((-a_inclination) * left_frame);
a_inclination = -a_inclination;
origin_coordinateY = a_inclination * left_frame + b_intercept;
if(target_coordinateX < right_frame){
}
else{
//左側の枠から右側の枠に当たるときのY座標
target_coordinateY = a_inclination * right_frame + b_intercept;
}
}
else if(right_frame < target_coordinateX){ //右側の枠での跳ね返り後の軌跡。右枠側平均
target_coordinateX = 2 * right_frame - target_coordinateX;
b_intercept += 2 * (a_inclination * right_frame);
a_inclination= -a_inclination;
//cvLine(show_img, cvPoint(right_frame, b_intercept), cvPoint((int)target_coordinateX, target_destanceY), cvScalar(0,0,255), 2);
origin_coordinateY = a_inclination * right_frame + b_intercept;
if(left_frame < target_coordinateX){
}
else{
//右側の枠から左側の枠に当たるときのY座標
target_coordinateY = a_inclination * left_frame + b_intercept;
}
}
rebound_num++;
if(rebound_max < rebound_num){
//跳ね返りが多すぎる時は、中央を指定
target_coordinateX = (lower_right_f.x + lower_right_g.x + lower_left_f.x + lower_left_g.x)/4;
break;
}
}
if(target_coordinateX < center_line && center_line < gX_now_mallet){
target_direction = 0;
gpioPWM(25, 128);
closest_frequency = gpioSetPWMfrequency(25, 1000);
}
else if(center_line < target_coordinateX && gX_now_mallet < center_line){
target_direction = 1;
gpioPWM(25, 128);
closest_frequency = gpioSetPWMfrequency(25, 1000);
}
else{
gpioPWM(25, 0);
closest_frequency = gpioSetPWMfrequency(25, 0);
}
}
printf("a_inclination: %f\n",a_inclination);
printf("b_intercept: %f\n",b_intercept);
}
if(target_direction != -1){
gpioWrite(18, target_direction);
}
//pwm output for rotate
//台の揺れを想定してマージンをとる
/*if(abs(gX_after - gX_before) <= 1){//パックが動いてない場合一時停止
gpioPWM(25, 0);
closest_frequency = gpioSetPWMfrequency(25, 0);
a_inclination = 0;
b_intercept=0;
}
else if(gY_after-1 < gY_before ){ //packが離れていく時、台の中央に戻る
a_inclination = 0;
b_intercept=0;
//目標値は中央。台のロボット側(4点)からを計算
double center_line = (lower_right_f.x + lower_right_g.x + lower_left_f.x + lower_left_g.x)/4;
if(center_line + 3 < gX_now_mallet){ //+1 マージン
gpioPWM(25, 128);
closest_frequency = gpioSetPWMfrequency(25, 1500);
target_direction = 0;//反時計回り
}
else if(gX_now_mallet < center_line-3){ //-1 マージン
gpioPWM(25, 128);
closest_frequency = gpioSetPWMfrequency(25, 1500);
target_direction = 1;//時計回り
}
else{
gpioPWM(25, 0);
closest_frequency = gpioSetPWMfrequency(25, 0);
}
}
else{
gpioPWM(25, 128);
closest_frequency = gpioSetPWMfrequency(25, 1500);
a_inclination = (gY_after - gY_before) / (gX_after - gX_before);
b_intercept = gY_after - a_inclination * gX_after;
//一次関数で目標X座標の計算
if(a_inclination){
target_coordinateX = (int)((target_destanceY - b_intercept) / a_inclination);
}
else{
target_coordinateX = 0;
}
}
printf("a_inclination: %f\n",a_inclination);
printf("b_intercept: %f\n",b_intercept);
int left_frame = (upper_left_f.x + lower_left_f.x)/2;
int right_frame = (upper_right_f.x + lower_right_f.x)/2;
origin_coordinateY = a_inclination * left_frame + b_intercept;
if(target_coordinateX < left_frame){
cvLine(show_img, cvPoint((int)gX_after, (int)gY_after), cvPoint(left_frame, origin_coordinateY), cvScalar(0,255,255), 2);
}
else if(right_frame < target_coordinateX){
cvLine(show_img, cvPoint((int)gX_after, (int)gY_after), cvPoint(right_frame, origin_coordinateY), cvScalar(0,255,255), 2);
}
else{
cvLine(show_img, cvPoint((int)gX_after, (int)gY_after), cvPoint((int)target_coordinateX, target_destanceY), cvScalar(0,255,255), 2);
}
int rebound_max = 5;
int rebound_num = 0;
while(target_coordinateX < left_frame || right_frame < target_coordinateX){
if(target_coordinateX < left_frame){ //左側の枠での跳ね返り後の軌跡。左枠側平均
target_coordinateX = 2 * left_frame - target_coordinateX;
b_intercept -= 2 * ((-a_inclination) * left_frame);
a_inclination = -a_inclination;
origin_coordinateY = a_inclination * left_frame + b_intercept;
if(target_coordinateX < right_frame){
cvLine(show_img, cvPoint(left_frame, origin_coordinateY), cvPoint((int)target_coordinateX, target_destanceY), cvScalar(0,255,255), 2);
}
else{
//左側の枠から右側の枠に当たるときのY座標
target_coordinateY = a_inclination * right_frame + b_intercept;
cvLine(show_img, cvPoint(left_frame, origin_coordinateY), cvPoint(right_frame, target_coordinateY), cvScalar(0,255,255), 2);
}
}
else if(right_frame < target_coordinateX){ //右側の枠での跳ね返り後の軌跡。右枠側平均
target_coordinateX = 2 * right_frame - target_coordinateX;
b_intercept += 2 * (a_inclination * right_frame);
a_inclination= -a_inclination;
//cvLine(show_img, cvPoint(right_frame, b_intercept), cvPoint((int)target_coordinateX, target_destanceY), cvScalar(0,0,255), 2);
origin_coordinateY = a_inclination * right_frame + b_intercept;
if(left_frame < target_coordinateX){
cvLine(show_img, cvPoint(right_frame, origin_coordinateY), cvPoint((int)target_coordinateX, target_destanceY), cvScalar(0,255,255), 2);
}
else{
//右側の枠から左側の枠に当たるときのY座標
target_coordinateY = a_inclination * left_frame + b_intercept;
cvLine(show_img, cvPoint(right_frame, origin_coordinateY), cvPoint(left_frame, target_coordinateY), cvScalar(0,255,255), 2);
}
}
rebound_num++;
if(rebound_max < rebound_num){
//跳ね返りが多すぎる時は、中央を指定
target_coordinateX = (lower_right_f.x + lower_right_g.x + lower_left_f.x + lower_left_g.x)/4;
break;
}
}
printf("target_coordinateX: %d\n",target_coordinateX);
//防御ラインの描画
cvLine(show_img, cvPoint(CAM_PIX_WIDTH, target_destanceY), cvPoint(0, target_destanceY), cvScalar(255,255,0), 2);
//マレットの動きの描画
cvLine(show_img, cvPoint((int)gX_now_mallet, (int)gY_now_mallet), cvPoint((int)target_coordinateX, target_destanceY), cvScalar(0,0,255), 2);
//cvPutText (show_img, to_c_char((int)gX_now_mallet), cvPoint(460,30), &font, cvScalar(220,50,50));
//cvPutText (show_img, to_c_char((int)target_coordinateX), cvPoint(560,30), &font, cvScalar(50,220,220));
int amount_movement = target_coordinateX - gX_now_mallet;
//reacted limit-switch and target_direction rotate
// if(gpioRead(6) == 1){//X軸右
// gpioPWM(25, 128);
// closest_frequency = gpioSetPWMfrequency(25, 1500);
// target_direction = 0;//反時計回り
// printf("X軸右リミット！反時計回り\n");
// }
// else
if(gpioRead(26) == 1){//X軸左
gpioPWM(25, 128);
closest_frequency = gpioSetPWMfrequency(25, 1500);
target_direction = 1;//時計回り
printf("X軸左リミット！時計回り\n");
}
else if(gpioRead(5) == 1){//Y軸右上
gpioPWM(23, 128);
gpioSetPWMfrequency(23, 1500);
gpioWrite(14, 0);
printf("Y軸右上リミット！時計回り\n");
}
else if(gpioRead(13) == 1){//Y軸右下
gpioPWM(23, 128);
gpioSetPWMfrequency(23, 1500);
gpioWrite(14, 1);
printf("Y軸右下リミット！反時計回り\n");
}
else if(gpioRead(19) == 1){//Y軸左下
gpioPWM(24, 128);
gpioSetPWMfrequency(24, 1500);
gpioWrite(15, 0);
printf("Y軸左下リミット！時計回り\n");
}
else if(gpioRead(21) == 1){//Y軸左上
gpioPWM(24, 0);
gpioSetPWMfrequency(24, 1500);
gpioWrite(15, 1);
printf("Y軸左上リミット！反時計回り\n");
}
else{
//Y軸固定のため
gpioSetPWMfrequency(23, 0);
gpioSetPWMfrequency(24, 0);
if(amount_movement > 0){
target_direction = 1;//時計回り
}
else if(amount_movement < 0){
target_direction = 0;//反時計回り
}
}
if(target_direction != -1){
gpioWrite(18, target_direction);
}
else{
gpioPWM(24, 0);
gpioSetPWMfrequency(24, 0);
}
printf("setting_frequency: %d\n", closest_frequency);*/
// 指定したウィンドウ内に画像を表示する
//cvShowImage("Previous Image", img_all_round2);
// cvShowImage("Now Image", show_img);
// cvShowImage("pack", dst_img_pack);
// cvShowImage("mallet", dst_img_mallet);
// cvShowImage ("Poly", poly_dst);
cvReleaseImage (&dst_img_mallet);
cvReleaseImage (&dst_img_pack);
cvReleaseImage (&dst_img2_mallet);
cvReleaseImage (&dst_img2_pack);
if(cv::waitKey(1) >= 0) {
break;
}
}
else{ //リセット信号が来た場合
is_pushed_decision_button = 0;
}
}
gpioTerminate();
cvDestroyAllWindows();
//Clean up used CvCapture*
cvReleaseCapture(&capture_robot_side);
cvReleaseCapture(&capture_human_side);
//Clean up used images
cvReleaseImage(&poly_dst);
cvReleaseImage(&tracking_img);
cvReleaseImage(&img_all_round);
cvReleaseImage(&img_human_side);
cvReleaseImage(&img_all_round2);
cvReleaseImage(&show_img);
cvReleaseImage(&img_robot_side);
return 0;
}
int main(int argc, char **argv)
{
pid_t pid;
int flags;
/* check command line parameters */
initOpts(argc, argv);
if (!foreground) {
/* Fork off the parent process */
pid = fork();
if (pid < 0) { exit(EXIT_FAILURE); }
/* If we got a good PID, then we can exit the parent process. */
if (pid > 0) { exit(EXIT_SUCCESS); }
/* Change the file mode mask */
umask(0);
/* Open any logs here */
/* NONE */
/* Create a new SID for the child process */
if (setsid() < 0) fatal("setsid failed (%m)");
/* Change the current working directory */
if ((chdir("/")) < 0) fatal("chdir failed (%m)");
/* Close out the standard file descriptors */
fclose(stdin);
fclose(stdout);
}
/* configure library */
gpioCfgBufferSize(bufferSizeMilliseconds);
gpioCfgClock(clockMicros, clockPeripheral, 0);
gpioCfgInterfaces(ifFlags);
gpioCfgDMAchannels(DMAprimaryChannel, DMAsecondaryChannel);
gpioCfgSocketPort(socketPort);
gpioCfgMemAlloc(memAllocMode);
if (updateMaskSet) gpioCfgPermissions(updateMask);
gpioCfgNetAddr(numSockNetAddr, sockNetAddr);
gpioCfgSetInternals(cfgInternals);
/* start library */
if (gpioInitialise()< 0) fatal("Can't initialise pigpio library");
/* create pipe for error reporting */
unlink(PI_ERRFIFO);
mkfifo(PI_ERRFIFO, 0664);
if (chmod(PI_ERRFIFO, 0664) < 0)
fatal("chmod %s failed (%m)", PI_ERRFIFO);
errFifo = freopen(PI_ERRFIFO, "w+", stderr);
if (errFifo)
{
/* set stderr non-blocking */
flags = fcntl(fileno(errFifo), F_GETFL, 0);
fcntl(fileno(errFifo), F_SETFL, flags | O_NONBLOCK);
/* request SIGHUP/SIGTERM from libarary for termination */
gpioSetSignalFunc(SIGHUP, terminate);
gpioSetSignalFunc(SIGTERM, terminate);
/* sleep forever */
while (1)
{
/* cat /dev/pigerr to view daemon errors */
sleep(5);
fflush(errFifo);
}
}
else
{
fprintf(stderr, "freopen failed (%m)");
gpioTerminate();
}
return 0;
}
int main(int argc, char* argv[] )
{
int i;
pthread_t *pS0Thread[NUM_GPIOS];
pthread_t *pW1Thread;
cfg_t *cfg;
char logBuffer[LOG_BUFFER_SIZE+1];
cfg_opt_t opts[] = {
CFG_SIMPLE_FLOAT("thermal_coefficient", &confOptions.termCoefficent),
CFG_SIMPLE_INT("flow_per_hour", &confOptions.literAnHour),
CFG_SIMPLE_INT("log_level", &confOptions.logLevel),
CFG_SIMPLE_INT("temp_reader_delay", &confOptions.tempReaderDelay),
CFG_SIMPLE_INT("temp_reader_interval", &confOptions.tempReaderIntval),
CFG_SIMPLE_BOOL("push_bullet_notification", &confOptions.bulletNotificationEnabled),
CFG_SIMPLE_BOOL("email_notification", &confOptions.emailNotificationEnabled),
CFG_SIMPLE_STR("emoncms_api_key", &confOptions.emonApiKey),
CFG_SIMPLE_BOOL("client_watchdog", &confOptions.watchdogEnabled),
CFG_SIMPLE_INT("gpio_4_s0_in_use", &confOptions.numGPIO4S0InUse),
CFG_SIMPLE_STR("therm_id_vorlauf", &confOptions.thermIdVorlauf),
CFG_SIMPLE_STR("therm_id_ruecklauf", &confOptions.thermIdRuecklauf),
CFG_SIMPLE_INT("oil_consumption_per_hour", &confOptions.oilConsumptionPerHour),
CFG_END()
};
confOptions.run_mode = DEFAULT_RUN_MODE;
confOptions.termCoefficent = CONF_DEFAULT_THERMCOEFF;
confOptions.literAnHour = CONF_DEFAULT_LITERANHOUR;
confOptions.logLevel = CONF_DEFAULT_LOGLEVEL;
confOptions.tempReaderDelay = CONF_DEFAULT_READERDELAY;
confOptions.tempReaderIntval = CONF_DEFAULT_READERINTVAL;
confOptions.bulletNotificationEnabled = CONF_DEFAULT_BULLETNOTICICATION;
confOptions.emailNotificationEnabled = CONF_DEFAULT_EMAILNOTICICATION;
confOptions.emonApiKey = CONF_DEFAULT_APIKEY;
confOptions.watchdogEnabled = CONF_DEFAULT_WATCHDOGENABLED;
confOptions.numGPIO4S0InUse = CONF_DEFAULT_NUMS0PINS;
confOptions.thermIdVorlauf = strdup(CONF_DEFAULT_THERMIDVORLAUF);
confOptions.thermIdRuecklauf = strdup(CONF_DEFAULT_THERMIDRUECKLAUF);
if( (cfg = cfg_init(opts, 0)) != NULL )
{
cfg_parse(cfg, CONF_FILE_PATH);
#ifdef DEBUG_ALL
printf("thermal_coefficient ....: %f\n", confOptions.termCoefficent);
printf("flow_per_hour ..........: %ld\n", confOptions.literAnHour);
printf("log_level ..............: %ld\n", confOptions.logLevel);
printf("temp_reader_delay ......: %ld\n", confOptions.tempReaderDelay);
printf("temp_reader_interval ...: %ld\n", confOptions.tempReaderIntval);
printf("push_bullet_notification: %s\n", confOptions.bulletNotificationEnabled?"true":"false");
printf("email_notification .....: %s\n", confOptions.emailNotificationEnabled?"true":"false");
printf("emoncms_api_key ........: %s\n", confOptions.emonApiKey?confOptions.emonApiKey:"NULL");
printf("client_watchdog ........: %s\n", confOptions.watchdogEnabled?"true":"false");
printf("gpio_4_s0_in_use .......: %ld\n", confOptions.numGPIO4S0InUse);
printf("therm_id_vorlauf .......: %s\n", confOptions.thermIdVorlauf?confOptions.thermIdVorlauf:"NULL");
printf("therm_id_ruecklauf .....: %s\n", confOptions.thermIdRuecklauf?confOptions.thermIdRuecklauf:"NULL");
printf("oil_consumption_per_hour: %ld\n", confOptions.oilConsumptionPerHour);
#endif // DEBUG_ALL
}
else
{
exit(3);
}
mainEndFlag = 0;
if( confOptions.emonApiKey != NULL && confOptions.thermIdVorlauf != NULL &&
confOptions.thermIdRuecklauf != NULL && confOptions.tempReaderDelay <= READER_DELAY_MAX &&
confOptions.tempReaderDelay >= 0 )
{
if (gpioInitialise() < 0)
{
// pigpio initialisation failed.
fprintf(stderr, "cannot initialize pigpio!\n");
return(1);
}
else
{
signal( SIGINT, sig_handler );
signal( SIGQUIT, sig_handler );
signal( SIGKILL, sig_handler );
signal( SIGTERM, sig_handler );
signal( SIGALRM, alrm_handler );
// pigpio initialisation okay ...
for( i = 0; i < NUM_GPIOS; i++)
{
#ifdef DEBUG_ALL
printf("Index %d\n", i);
#endif // DEBUG_ALL
gpioSetMode( gpioInUse[i].bcm_gpio, PI_INPUT ); // set GPIO as an input
gpioSetPullUpDown( gpioInUse[i].bcm_gpio, PI_PUD_DOWN); // activate internal pull-up
// create thread for this gpio
pS0Thread[i] = gpioStartThread(tickerThread, (void*) &gpioInUse[i]);
// gpioSetAlertFunc( gpioInUse[i].bcm_gpio, ticker[i] ); // set event handler "ticker"
}
if( (pW1Thread = gpioStartThread(W1ThermThread, NULL)) == NULL )
{
fprintf(stderr, "W1Thread failed to start ...\n");
for( i = 0; i < NUM_GPIOS; i++)
{
if( pS0Thread[i] != NULL )
{
gpioStopThread( pS0Thread[i] );
}
}
gpioTerminate();
}
else
{
while( !mainEndFlag )
{
gpioSleep(PI_TIME_RELATIVE, 0, 10000); // hold 1/100 sec high level
}
for( i = 0; i < NUM_GPIOS; i++)
{
if( pS0Thread[i] != NULL )
{
gpioStopThread( pS0Thread[i] );
}
}
gpioStopThread( pW1Thread );
gpioTerminate();
cfg_free(cfg);
if(confOptions.emonApiKey)
{
free(confOptions.emonApiKey);
}
if(confOptions.thermIdVorlauf)
{
free(confOptions.thermIdVorlauf);
}
if(confOptions.thermIdRuecklauf)
{
free(confOptions.thermIdRuecklauf);
}
}
}
}
else
{
confFail();
}
return(0);
}
int main(int argc, char *argv[])
{
char fname[120];
FILE *fp; // for output log file
struct tm *tm;
struct tm * timeinfo;
struct timeval t1, deltaT;
char tod[80]; // time of day string
char tbuf[80];
int millisec;
float deltaSec = 0;
int retval = 0;
if (gpioInitialise()<0) {
printf("Cannot initialize GPIO, quitting.\n");
return 1;
}
fresh = FALSE; // start out with no GPIO level changes yet seen
gettimeofday(&t1, NULL); // start time
if ((tm = localtime(&t1.tv_sec)) == NULL) return -1;
strftime(tod, sizeof tod, "%Y%m%d_%H%M%S.csv",tm);
strcpy(fname,BASENAME);
strncat(fname,tod,strlen(tod));
printf("Opening %s\n",fname);
fp = fopen(fname,"w");
if (fp == NULL) return -1;
gpioSetMode(SIGIN, PI_INPUT);
// gpioSetPullUpDown(SIGIN, PI_PUD_UP);
gpioSetPullUpDown(SIGIN, PI_PUD_DOWN);
gpioSetAlertFunc(SIGIN, alert);
printf("# Motion log start...\n");
do {
if (fresh == TRUE) {
fresh = FALSE;
timeval_subtract (&deltaT, &t2, &t1);
if (g_level == 1) { // is this rising edge (start of new motion event?)
t1.tv_sec = t2.tv_sec; // save time structure values for next time
t1.tv_usec = t2.tv_usec;
}
millisec = lrint(t2.tv_usec/1000.0); // round to nearest millisec
if (millisec>=1000) { // Allow for rounding up to nearest second
millisec -=1000;
t2.tv_sec++;
}
timeinfo = localtime(&t2.tv_sec);
strftime(tbuf, 80, "%Y-%m-%d %H:%M:%S",timeinfo);
fprintf(fp,"%s.%03d, ",tbuf,millisec); // write to file including milliseconds
if (g_level == 0) { // returned to zero; end of motion event
deltaSec = (g_tick-lastTick) / 1E6; // ticks in microseconds
fprintf(fp,"%d, %4.1f\n", g_level, deltaSec);
} else { // start of new motion event
deltaSec = deltaT.tv_sec + (deltaT.tv_usec/1.0E6);
fprintf(fp,"%d, %4.1f\n", g_level, deltaSec);
retval = system(STILLCAP); // command to trigger local capture of still image
retval |= system(SENDALARM); // notify other device of motion
}
lastTick = g_tick;
fflush(fp); // make sure it's written to the file
/*
if (g_level == 1) {
// capture still image
// raspistill -t 1000 -rot 180 -o test.jpg
tm = localtime(&t2.tv_sec);
strftime(tod, sizeof tod, "m_%Y%m%d_%H%M%S.jpg",tm);
strcpy(fname, "raspistill -t 1000 -rot 180 -o "); // command to record image
strcat(fname, tod);
printf("Saving %s\n", tod);
retval = system(fname);
}
*/
} // if fresh
sleep(1);
} while (1);
gpioTerminate(); // never reached
fclose(fp);
return retval;
}
int main (int argc, char **argv)
{
struct sigaction act;
char configFilePath[MAXPATHLEN];
char hostName[1024];
int c; // for getopt
int socket_fd_cms;
config_t cfg;
struct config config;
struct sockaddr_in info_cms;
struct hostent *he_cms;
// struct data data;
struct timeval time_last, time_now;
char tcp_buffer[1024];
int num;
// print help if no arguments given
if (argc==1)
{
printHelp();
return 1;
}
// init library
if (gpioInitialise()<0)
return 1;
// load and parse config file
opterr = 0;
while ((c=getopt(argc, argv, "c:")) != -1) {
switch (c) {
case 'c': strcpy(configFilePath, optarg); break;
case '?': if (optopt=='d' || optopt=='h')
fprintf(stderr, "Option -%c requires an argument.\n", optopt);
else if (isprint (optopt))
fprintf(stderr, "Unknown option '-%c'.\n", optopt);
else
fprintf(stderr, "Unknown option character '\\x%x'.\n", optopt);
gpioTerminate();
return 1;
default: gpioTerminate(); abort();
}
}
config_init(&cfg);
// Read the file. If there is an error, report it and exit.
if(! config_read_file(&cfg, configFilePath))
{
fprintf(stderr, "%s:%d - %s\n", config_error_file(&cfg),
config_error_line(&cfg), config_error_text(&cfg));
config_destroy(&cfg);
gpioTerminate();
return(EXIT_FAILURE);
}
// node, host, apikey
if (!(config_lookup_string(&cfg, "node", &(config.pNodeName))))
{
fprintf(stderr, "No 'node' setting in configuration file.\n");
config_destroy(&cfg);
gpioTerminate();
return(EXIT_FAILURE);
}
if (!(config_lookup_string(&cfg, "host", &(config.pHostName))))
{
fprintf(stderr, "No 'host' setting in configuration file.\n");
config_destroy(&cfg);
gpioTerminate();
return(EXIT_FAILURE);
}
if (!(config_lookup_string(&cfg, "apikey", &(config.pApiKey))))
{
fprintf(stderr, "No 'apikey' setting in configuration file.\n");
config_destroy(&cfg);
gpioTerminate();
return(EXIT_FAILURE);
}
if (!config.pHostName[0])
{
printf("No host name found. Did you specify the '-h hostname' option or mention it in the config file?\n");
config_destroy(&cfg);
gpioTerminate();
return EX_UNAVAILABLE;
}
if (!(he_cms = gethostbyname(config.pHostName)))
{
fprintf(stderr, "Could not resolve host name '%s', err %d.\n", config.pHostName, h_errno);
config_destroy(&cfg);
gpioTerminate();
exit(1);
}
info_cms.sin_family = AF_INET;
info_cms.sin_port = htons(80);
info_cms.sin_addr = *((struct in_addr *)he_cms->h_addr);
/* if ((socket_fd_cms = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP)) < 0)
{
fprintf(stderr, "Could not allocate socket, err %d.\n", errno);
config_destroy(&cfg);
gpioTerminate();
exit(1);
}
if (connect(socket_fd_cms, (struct sockaddr *)&info_cms, sizeof(struct sockaddr)) < 0)
{
fprintf(stderr, "Could not connect to server, err%d.\n", errno);
close (socket_fd_cms);
config_destroy(&cfg);
gpioTerminate();
exit(1);
}
*/
// config GPIO
gpioSetMode (GPIO, PI_INPUT);
gpioSetPullUpDown (GPIO, PI_PUD_UP);
gpioSetAlertFunc (GPIO, myAlert);
act.sa_handler = intHandler;
sigaction(SIGINT, &act, NULL); // catch Ctrl-C
sigaction(SIGTERM, &act, NULL); // catch kill
gettimeofday(&time_last, NULL);
gettimeofday(&time_now, NULL);
unsigned long elapsedtime_us; // [us]
float elapsedtime_s; // [s]
float power; // [W]
syslog(LOG_INFO, "started. waiting for pulse...");
while (!clean_up)
{
pause();
if (!clean_up) // to avoid running even once if SIGINT arrives during pause()
{
// printf ("back.\n"); fflush(stdout);
gettimeofday(&time_now, NULL);
elapsedtime_us = (time_now.tv_sec - time_last.tv_sec)*1000000 - time_last.tv_usec + time_now.tv_usec;
printf("elapsed microseconds: %lu\n", elapsedtime_us);
elapsedtime_s = elapsedtime_us / 1000000.0;
printf("elapsed seconds: %f\n", elapsedtime_s);
power = 1.0 * 3600.0 / elapsedtime_s; // 1.0Wh per pulse * 3600 s/h / seconds = Whs/hs = W
printf("current power: %f\n", power);
syslog(LOG_INFO, "elapsed seconds: %f, current power: %f", elapsedtime_s, power);
if (power > 10000.0)
{
syslog(LOG_INFO, "error: power >10kW.");
}
else
{
time_last.tv_sec = time_now.tv_sec;
time_last.tv_usec = time_now.tv_usec;
// generate json string for emonCMS
sprintf (tcp_buffer, "GET /input/post.json?node=\"%s\"&json={pulseBoiler:1,powerBoiler:%5.3f}&apikey=%s HTTP/1.1\r\nHost: %s\r\nUser-Agent: %s %s\r\nConnection: keep-alive\r\n\r\n", config.pNodeName, power, config.pApiKey, config.pHostName, TOOLNAME, RASPI_PULSECOUNT_VERSION);
printf ("-----\nbuflen: %ld\n%s\n", strlen(tcp_buffer), tcp_buffer);
if ((socket_fd_cms = socket(AF_INET, SOCK_STREAM, IPPROTO_TCP)) < 0)
{
fprintf(stderr, "Could not allocate socket, err %d.\n", errno);
}
else
{
if (connect(socket_fd_cms, (struct sockaddr *)&info_cms, sizeof(struct sockaddr)) < 0) // re-connect each time
{
fprintf(stderr, "Could not connect to server, err %d.\n", errno);
syslog(LOG_WARNING,"Could not connect to server, err %d (%s).", errno, strerror(errno));
}
else
{
num=send(socket_fd_cms, tcp_buffer, strlen(tcp_buffer), 0);
if (num <0)
printf ("send error: %d\n", errno);
else
printf ("sent: %ld\n", num);
}
close (socket_fd_cms);
}
}
}
}
// clean up (not usually executed until Ctrl-C)
printf ("clean up.\n");
syslog(LOG_INFO, "clean up.");
close (socket_fd_cms);
config_destroy(&cfg);
gpioTerminate();
return (EX_OK);
}
int main(int argc, char* argv[]) {
int counter;
wchar_t auxstr[20];
printf("start!\n");
printf("PUCK MINH: %d\n",minH);
printf("PUCK MAXH: %d\n",maxH);
printf("PUCK MINS: %d\n",minS);
printf("PUCK MAXS: %d\n",maxS);
printf("PUCK MINV: %d\n",minV);
printf("PUCK MAXV: %d\n",maxV);
printf("ROBOT MINH: %d\n",RminH);
printf("ROBOT MAXH: %d\n",RmaxH);
printf("ROBOT MINS: %d\n",RminS);
printf("ROBOT MAXS: %d\n",RmaxS);
printf("ROBOT MINV: %d\n",RminV);
printf("ROBOT MAXV: %d\n",RmaxV);
printf("FPS: %d\n",fps);
//pwm initialize
if(gpioInitialise() < 0) return 1;
// 読み込み画像ファイル名
char imgfile[] = "camera/photodir/capmallet1.png";
char imgfile2[] = "camera/photodir/capmallet2.png";
// 画像の読み込み
img = cvLoadImage(imgfile, CV_LOAD_IMAGE_ANYCOLOR | CV_LOAD_IMAGE_ANYDEPTH);
img2 = cvLoadImage(imgfile2, CV_LOAD_IMAGE_ANYCOLOR | CV_LOAD_IMAGE_ANYDEPTH);
// 画像の表示用ウィンドウ生成
cvNamedWindow("circle_sample", CV_WINDOW_AUTOSIZE);
cvNamedWindow("circle_sample2", CV_WINDOW_AUTOSIZE);
//cvNamedWindow("cv_ColorExtraction");
// Init font
cvInitFont(&font,CV_FONT_HERSHEY_SIMPLEX|CV_FONT_ITALIC, 0.4,0.4,0,1);
IplImage* dst_img_mallett = cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 3);
IplImage* dst_img_pack = cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 3);
IplImage* dst_img2_mallett = cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 3);
IplImage* dst_img2_pack = cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 3);
//白抽出0,255,0,15,240,255
//黒抽出0, 255, 0, 50, 0, 100
//青検出0, 255, 50, 200, 100, 180
//cv_ColorExtraction(img, dst_img_mallett, CV_BGR2HSV, 0, 255, 50, 200, 100, 180);
cv_ColorExtraction(img, dst_img_pack, CV_BGR2HSV, 0, 255, 0, 50, 0, 100);
cv_ColorExtraction(img2, dst_img2_mallett, CV_BGR2HSV, 0, 255, 50, 200, 100, 180);
cv_ColorExtraction(img2, dst_img2_pack, CV_BGR2HSV, 0, 255, 0, 50, 0, 100);
//CvMoments moment_mallett;
CvMoments moment_pack;
CvMoments moment2_mallett;
CvMoments moment2_pack;
//cvSetImageCOI(dst_img_mallett, 1);
cvSetImageCOI(dst_img_pack, 1);
cvSetImageCOI(dst_img2_mallett, 1);
cvSetImageCOI(dst_img2_pack, 1);
//cvMoments(dst_img_mallett, &moment_mallett, 0);
cvMoments(dst_img_pack, &moment_pack, 0);
cvMoments(dst_img2_mallett, &moment2_mallett, 0);
cvMoments(dst_img2_pack, &moment2_pack, 0);
//座標計算
double m00_before = cvGetSpatialMoment(&moment_pack, 0, 0);
double m10_before = cvGetSpatialMoment(&moment_pack, 1, 0);
double m01_before = cvGetSpatialMoment(&moment_pack, 0, 1);
double m00_after = cvGetSpatialMoment(&moment2_pack, 0, 0);
double m10_after = cvGetSpatialMoment(&moment2_pack, 1, 0);
double m01_after = cvGetSpatialMoment(&moment2_pack, 0, 1);
double gX_before = m10_before/m00_before;
double gY_before = m01_before/m00_before;
double gX_after = m10_after/m00_after;
double gY_after = m01_after/m00_after;
double m00_mallett = cvGetSpatialMoment(&moment2_mallett, 0, 0);
double m10_mallett = cvGetSpatialMoment(&moment2_mallett, 1, 0);
double m01_mallett = cvGetSpatialMoment(&moment2_mallett, 0, 1);
double gX_now_mallett = m10_mallett/m00_mallett;
double gY_now_mallett = m01_mallett/m00_mallett;
cvCircle(img2, cvPoint((int)gX_before, (int)gY_before), 50, CV_RGB(0,0,255), 6, 8, 0);
cvLine(img2, cvPoint((int)gX_before, (int)gY_before), cvPoint((int)gX_after, (int)gY_after), cvScalar(0,255,0), 2);
int target_destanceY = 480 - 30;//Y座標の距離を一定にしている。ディフェンスライン。
//パックの移動は直線のため、一次関数の計算を使って、その後の軌跡を予測する。
double a_inclination = (gY_after - gY_before) / (gX_after - gX_before);
double b_intercept = gY_after - a_inclination * gX_after;
printf("gX_after: %f\n",gX_after);
printf("gY_after: %f\n",gY_after);
printf("gX_before: %f\n",gX_before);
printf("gY_before: %f\n",gY_before);
printf("a_inclination: %f\n",a_inclination);
printf("b_intercept: %f\n",b_intercept);
int target_coordinateX = (int)((target_destanceY - b_intercept) / a_inclination);
printf("target_coordinateX: %d\n",target_coordinateX);
cvLine(img2, cvPoint((int)gX_after, (int)gY_after), cvPoint((int)target_coordinateX, target_destanceY), cvScalar(0,255,255), 2);
cvLine(img2, cvPoint(640, target_destanceY), cvPoint(0, target_destanceY), cvScalar(255,255,0), 2);
cvLine(img2, cvPoint((int)gX_now_mallett, (int)gY_now_mallett), cvPoint((int)target_coordinateX, target_destanceY), cvScalar(0,0,255), 2);
cvPutText (img2, to_c_char((int)gX_now_mallett), cvPoint(460,30), &font, cvScalar(220,50,50));
cvPutText (img2, to_c_char((int)target_coordinateX), cvPoint(560,30), &font, cvScalar(50,220,220));
int amount_movement = gX_now_mallett - target_coordinateX;
//2枚の画像比較1回で移動できる量の計算
int max_amount_movement = CAM_PIX_WIDTH * 0.54 / 1; //CAM_PIX_WIDTH:640, 比較にかかる時間:0.27*2, 端までの移動時間：1s
int target_direction;
if(amount_movement > 0){
if(max_amount_movement < amount_movement){
amount_movement = max_amount_movement;
}
target_direction = 0;//時計回り
}
else if(amount_movement < 0){
amount_movement = -amount_movement;//正の数にする
if(max_amount_movement < amount_movement){
amount_movement = max_amount_movement;
}
target_direction = 1;//反時計回り
}
//pwm output
double set_time_millis= 270 * amount_movement / max_amount_movement;//0.27ms*(0~1)
gpioSetMode(18, PI_OUTPUT);
gpioSetMode(19, PI_OUTPUT);
gpioPWM(18, 128);
gpioWrite(19, target_direction);
int closest_frequency = gpioSetPWMfrequency(18, 2000);
printf("setting_frequency: %d\n", closest_frequency);
gpioSetTimerFunc(0, (int)set_time_millis, pwmReset);
// 指定したウィンドウ内に画像を表示する
cvShowImage("circle_sample", img);
cvShowImage("circle_sample2", img2);
while(1) {
if(cv::waitKey(30) >= 0) {
break;
}
}
gpioTerminate();
//Clean up used images
cvReleaseImage(&img);
// cvReleaseImage (&dst_img);
cvDestroyAllWindows() ;
return 0;
}
int main(int argc, char* argv[]) {
int counter;
wchar_t auxstr[20];
printf("start!\n");
printf("PUCK MINH: %d\n",minH);
printf("PUCK MAXH: %d\n",maxH);
printf("PUCK MINS: %d\n",minS);
printf("PUCK MAXS: %d\n",maxS);
printf("PUCK MINV: %d\n",minV);
printf("PUCK MAXV: %d\n",maxV);
printf("ROBOT MINH: %d\n",RminH);
printf("ROBOT MAXH: %d\n",RmaxH);
printf("ROBOT MINS: %d\n",RminS);
printf("ROBOT MAXS: %d\n",RmaxS);
printf("ROBOT MINV: %d\n",RminV);
printf("ROBOT MAXV: %d\n",RmaxV);
printf("FPS: %d\n",fps);
// 読み込み画像ファイル名
char imgfile[] = "camera/photodir/capmallet1.png";
char imgfile2[] = "camera/photodir/capmallet2.png";
// 画像の読み込み
img = cvLoadImage(imgfile, CV_LOAD_IMAGE_ANYCOLOR | CV_LOAD_IMAGE_ANYDEPTH);
img2 = cvLoadImage(imgfile2, CV_LOAD_IMAGE_ANYCOLOR | CV_LOAD_IMAGE_ANYDEPTH);
// 画像の表示用ウィンドウ生成
cvNamedWindow("circle_sample", CV_WINDOW_AUTOSIZE);
cvNamedWindow("circle_sample2", CV_WINDOW_AUTOSIZE);
//cvNamedWindow("cv_ColorExtraction");
// Init font
cvInitFont(&font,CV_FONT_HERSHEY_SIMPLEX|CV_FONT_ITALIC, 0.4,0.4,0,1);
IplImage* dst_img_mallett = cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 3);
IplImage* dst_img_pack = cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 3);
IplImage* dst_img2_mallett = cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 3);
IplImage* dst_img2_pack = cvCreateImage(cvGetSize(img), IPL_DEPTH_8U, 3);
//白抽出0,255,0,15,240,255
//黒抽出0, 255, 0, 50, 0, 100
//青検出0, 255, 50, 200, 100, 180
//cv_ColorExtraction(img, dst_img_mallett, CV_BGR2HSV, 0, 255, 50, 200, 100, 180);
cv_ColorExtraction(img, dst_img_pack, CV_BGR2HSV, 0, 255, 0, 50, 0, 100);
cv_ColorExtraction(img2, dst_img2_mallett, CV_BGR2HSV, 0, 255, 50, 200, 100, 180);
cv_ColorExtraction(img2, dst_img2_pack, CV_BGR2HSV, 0, 255, 0, 50, 0, 100);
//CvMoments moment_mallett;
CvMoments moment_pack;
CvMoments moment2_mallett;
CvMoments moment2_pack;
//cvSetImageCOI(dst_img_mallett, 1);
cvSetImageCOI(dst_img_pack, 1);
cvSetImageCOI(dst_img2_mallett, 1);
cvSetImageCOI(dst_img2_pack, 1);
//cvMoments(dst_img_mallett, &moment_mallett, 0);
cvMoments(dst_img_pack, &moment_pack, 0);
cvMoments(dst_img2_mallett, &moment2_mallett, 0);
cvMoments(dst_img2_pack, &moment2_pack, 0);
//座標計算
double m00_before = cvGetSpatialMoment(&moment_pack, 0, 0);
double m10_before = cvGetSpatialMoment(&moment_pack, 1, 0);
double m01_before = cvGetSpatialMoment(&moment_pack, 0, 1);
double m00_after = cvGetSpatialMoment(&moment2_pack, 0, 0);
double m10_after = cvGetSpatialMoment(&moment2_pack, 1, 0);
double m01_after = cvGetSpatialMoment(&moment2_pack, 0, 1);
double gX_before = m10_before/m00_before;
double gY_before = m01_before/m00_before;
double gX_after = m10_after/m00_after;
double gY_after = m01_after/m00_after;
double m00_mallett = cvGetSpatialMoment(&moment2_mallett, 0, 0);
double m10_mallett = cvGetSpatialMoment(&moment2_mallett, 1, 0);
double m01_mallett = cvGetSpatialMoment(&moment2_mallett, 0, 1);
double gX_now_mallett = m10_mallett/m00_mallett;
double gY_now_mallett = m01_mallett/m00_mallett;
cvCircle(img2, cvPoint((int)gX_before, (int)gY_before), 50, CV_RGB(0,0,255), 6, 8, 0);
cvLine(img2, cvPoint((int)gX_before, (int)gY_before), cvPoint((int)gX_after, (int)gY_after), cvScalar(0,255,0), 2);
int target_destanceY = 480 - 30;//Y座標の距離を一定にしている。ディフェンスライン。
//パックの移動は直線のため、一次関数の計算を使って、その後の軌跡を予測する。
double a_inclination = (gY_after - gY_before) / (gX_after - gX_before);
double b_intercept = gY_after - a_inclination * gX_after;
printf("gX_after: %f\n",gX_after);
printf("gY_after: %f\n",gY_after);
printf("gX_before: %f\n",gX_before);
printf("gY_before: %f\n",gY_before);
printf("a_inclination: %f\n",a_inclination);
printf("b_intercept: %f\n",b_intercept);
int target_coordinateX = (int)((target_destanceY - b_intercept) / a_inclination);
printf("target_coordinateX: %d\n",target_coordinateX);
cvLine(img2, cvPoint((int)gX_after, (int)gY_after), cvPoint((int)target_coordinateX, target_destanceY), cvScalar(0,255,255), 2);
cvLine(img2, cvPoint(640, target_destanceY), cvPoint(0, target_destanceY), cvScalar(255,255,0), 2);
cvLine(img2, cvPoint((int)gX_now_mallett, (int)gY_now_mallett), cvPoint((int)target_coordinateX, target_destanceY), cvScalar(0,0,255), 2);
cvPutText (img2, to_c_char((int)gX_now_mallett), cvPoint(460,30), &font, cvScalar(220,50,50));
cvPutText (img2, to_c_char((int)target_coordinateX), cvPoint(560,30), &font, cvScalar(50,220,220));
// 指定したウィンドウ内に画像を表示する
cvShowImage("circle_sample", img);
cvShowImage("circle_sample2", img2);
//pwm output test
if(gpioInitialise() < 0) return 1;
gpioSetMode(18, PI_OUTPUT);
gpioPWM(18, 128);
gpioSetPWMfrequency(18, 1000);
/*if(wiringPiSetupGpio()==-1){
printf("cannotsetup gpio.\n" );
return 1;
}
pinMode(18,PWM_OUTPUT);
pwmSetMode(PWM_MODE_MS);
pwmSetClock(64);
pwmSetRange(100);
pwmWrite(18,50);*/
while(1) {
if(cv::waitKey(30) >= 0) {
break;
}
}
//Clean up used images
cvReleaseImage(&img);
// cvReleaseImage (&dst_img);
cvDestroyAllWindows() ;
return 0;
}
