int main(int argc, char *argv[]){
int gpio = atoi(argv[1]);
unsigned int i;
gpioExport(gpio);
gpioDirection(gpio, gpioDIRECTION_IN);
while(1){
printf("Read: %c\n", gpioRead(gpio));
for(i=0; i<9000000; ++i);
}
return 0;
}
int main(void)
{
int bankNumber;
int pin;
char direction[5];
int errorCheck;
int loop=1;
int pinValue;
int value;
gpioBank *bank;//Creates the pointer that will hold the structure to store all the address for the gpio bank
system("clear");
printf("%s\n", "What GPIO bank do you wish to set as input or output? 0, 1, 2, or 3: ");
fflush(stdout);
fscanf(stdin, "%ui1", &bankNumber);
fflush(stdin);
printf("\n%s\n", "What pin do you wish to edit or read the value of? 0-31: ");
fflush(stdout);
fscanf(stdin,"%ui2", &pin);
fflush(stdin);
printf("\n%s\n", "What direction do you wish to make this pin? in or out ");
fflush(stdout);
fscanf(stdin,"%s3", direction);
fflush(stdin);
printf("\n%s\n", "Should the pin be high(1) or low(0) if output (Ignored it not)?: ");
fflush(stdout);
fscanf(stdin,"%ui2", &value);
fflush(stdin);
printf("\n%s", "Here is what you typed");
printf("\n%s%u", "Bank:", bankNumber);
printf("\n%s%u", "Pin:", pin);
printf("\n%s%s", "Direction:", direction);
printf("\n%s%u\n", "Value to write:", value);
bank=gpioinit(bankNumber);
gpiodirection(bank, pin, direction);
if(strncmp("in",direction,2)==0)
{
pinValue=gpioRead(bank,pin);
printf("\n%s%u","Current Value:", pinValue);
while(loop==1)
{
printf("\n%s", "Would you like to read the value of the pin again? (1)Yes (0)No: ");
scanf("%u1", &loop);
pinValue=gpioRead(bank,pin);
printf("\n%s%u","Current Value:", pinValue);
}
}
if(strncmp("ou",direction,2)==0)
{
gpiowrite(bank, pin, value);
}
gpiodone(bank);//Don't forget to close the banks you aren't going to use.
return 0;
}
int getDistance(int unit){
int distance = -1;
//Write Trig Low
gpioWrite(trigPin,0);
//Wait 2 us
waitN(2000);
//Write Trig High
gpioWrite(trigPin,1);
//Wait 10 us
waitN(10000);
///Write trig LOW
gpioWrite(trigPin,0);
//When Echo goes HIGH, start timer
//printf("Waiting for a ping....\n");
int timeOut = 0;
while(gpioRead(echoPin)==0 & (timeOut < 100)){timeOut++;}
if(timeOut>=999){printf("BAD READ \N");}
else{
struct timespec start_time;
clock_gettime(CLOCK_PROCESS_CPUTIME_ID, &start_time);
long dT;
//When echo goes low, stop timer
timeOut=0;
//printf("Waiting for Pong\n");
while(gpioRead(echoPin)==1 & (timeOut<100)){timeOut++;}
if(timeOut>=999){
printf("BAD READ \N");
return -1;
}
else{
//######################################REP JOB#######################
static void testCallback(UA_Server *server, void *data) {
UA_LOG_INFO(logger, UA_LOGCATEGORY_USERLAND, "testcallback");
ButtonValue = gpioRead(22);
}
/**************************************************
* Do all the cool stuff
**************************************************/
int main()
{
unsigned char len = 0;
unsigned short posX = 0x0000;
unsigned short posY = 0x0000;
setup();
while(1)
{
if(0 == gpioRead()) // touchIt 'int' pin pulled low indicating new touch
{
// Generate crc
crc = data[0] = '#'; // Start
crc ^= data[1] = GET_POS; // Command
crc ^= data[2] = 0x00; // Data length
crc ^= data[3] = crc;
// Send command
len = 4;
if(write(file_i2c, data, len) != len)
{
printf("Failed to write to the i2c bus\n");
}
// Receive confirmation
len = 8;
if(read(file_i2c, data, len) != len)
{
printf("Failed to read from the i2c bus\n");
}
else
{
if(data[0] == '$')
{
crc = 0;
for(c=0;c<7;c++)
{
crc ^= data[c];
}
if(crc == data[7])
{
posX = data[3];
posX <<= 8;
posX += data[4];
posY = data[5];
posY <<= 8;
posY += data[6];
printf("x: %d\ty: %d\n", posX, posY);
fflush(stdout);
}
}
}
}
}
}
static void portAIsr(void)
{
if ( PORT_PCR(IRQ_PORT, IRQ_PIN) & PORT_ISF) {
// printf("TODO: Handle driver interrupts! \n");
PORT_PCR(IRQ_PORT, IRQ_PIN) |= PORT_ISF;
}
#if 0
if ( PORT_PCR(HALL_A_PORT, HALL_A_PIN) & PORT_ISF) {
PORT_PCR(HALL_A_PORT, HALL_A_PIN) |= PORT_ISF;
}
if ( PORT_PCR(HALL_B_PORT, HALL_B_PIN) & PORT_ISF) {
PORT_PCR(HALL_B_PORT, HALL_B_PIN) |= PORT_ISF;
}
if ( PORT_PCR(HALL_C_PORT, HALL_C_PIN) & PORT_ISF) {
PORT_PCR(HALL_C_PORT, HALL_C_PIN) |= PORT_ISF;
}
if (gpioRead(HALL_A_PORT, HALL_A_PIN)) {
value |= HALL_A_BIT;
}
if (gpioRead(HALL_B_PORT, HALL_B_PIN)) {
value |= HALL_B_BIT;
}
if (gpioRead(HALL_C_PORT, HALL_C_PIN)) {
value |= HALL_C_BIT;
}
if (value > 0 && value <= MAX_HALL_PHASE) {
int idx;
commutator.hallPosition = value;
if (0 && commutator.ready) {
idx = commutationStepFromHallPos[commutator.hallPosition];
//ftmSetOutputMask(FTM_0, commutator.commutationMask[idx], FALSE);
// ftmSetInvCtrl(FTM_0, commutator.bipolarCompliment[idx], TRUE);
}
} else {
/* Motor Position Fault! */
printf("FAULT VALUE %d \n", value);
gpioClear(FET_DRIVER_ENABLE_PORT, FET_DRIVER_ENABLE_PIN);
gpioClear(FET_DRIVER_RESET_PORT, FET_DRIVER_RESET_PIN);
updateFlags |= UPDATE_HALT;
}
#endif
return;
}
int getCM(int Sensor) {
int trig = Trigs[Sensor];
int echo = Echos[Sensor];
//Send Trig pulse
gpioWrite(trig, PI_HIGH);
time_sleep(0.00001);
gpioWrite(trig, PI_LOW);
//Wait for Echo start
while(gpioRead(echo) == PI_LOW);
//Wait for Echos end
long startTime = micros();
while(gpioRead(echo) == PI_HIGH && micros() - startTime < 5800 );
long travelTime = micros() - startTime;
//Get distance in cm
int distance = travelTime / 58;
return distance;
}
int checkINT(void)
{
//int retval = 0;
/*
if ((PIND & (1<<NIRQ)) == 0)
printf("INT == 0\n");
else
{
printf("INT == 1\n");
retval = 1;
}
*/
return gpioRead(IRQ_N);
}
int read_digital(int chan)
{
int d_chan;
if (chan == 0) {d_chan = 4;}
if (chan == 1) {d_chan = 17;}
if (chan == 2) {d_chan = 27;}
if (chan == 3) {d_chan = 22;}
if (chan == 4) {d_chan = 6;}
if (chan == 5) {d_chan = 13;}
if (chan == 6) {d_chan = 19;}
if (chan == 7) {d_chan = 26;}
int result = gpioRead(d_chan);
return result;
}
void to_tx_mode(uint8_t txData[])
{
unsigned char i;
writeRfm(0x07, 0x01); // To ready mode
//cbi(PORTD, RXANT);
gpioWrite(RXANT, 0);
//sbi(PORTD, TXANT);
gpioWrite(TXANT, 1);
usleep(50000);
//delay_ms(50);
writeRfm(0x08, 0x03); // FIFO reset
writeRfm(0x08, 0x00); // Clear FIFO
writeRfm(0x34, 64); // preamble = 64nibble
writeRfm(0x3E, 17); // packet length = 17bytes
for (i=0; i<17; i++)
{
writeRfm(0x7F, txData[i]); // send payload to the FIFO
}
writeRfm(0x05, 0x04); // enable packet sent interrupt
i = readRfm(0x03); // Read Interrupt status1 register
i = readRfm(0x04);
writeRfm(0x07, 9); // Start TX
while (gpioRead(IRQ_N) != 0)
;
//while ((PIND & (1<<NIRQ)) != 0)
// ; // need to check interrupt here
writeRfm(0x07, 0x01); // to ready mode
gpioWrite(RXANT, 0);
//cbi(PORTD, RXANT); // disable all interrupts
gpioWrite(TXANT, 0);
//cbi(PORTD, TXANT);
}
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;
}
/*
* Class: com_diozero_pigpioj_PigpioGpio
* Method: read
* Signature: (I)I
*/
JNIEXPORT jint JNICALL Java_com_diozero_pigpioj_PigpioGpio_read
(JNIEnv* env, jclass clz, jint gpio) {
return gpioRead(gpio);
}
int ADCController::GetChannelValue(const unsigned int &Channel)
{
unsigned int RX = 0, TX = 0;
if(!_Mutex.tryLock(300) || Channel > 7)//we must have exclusive access.
return -1;
/*
MCP3004/8 10-bit ADC 4/8 channels
1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17
SB SD D2 D1 D0 NA NA B9 B8 B7 B6, B5 B4 B3 B2 B1 B0
SB 1
SD 0=differential 1=single
D2/D1/D0 0-7 channel
*/
//Now we need to set up 5 bits of data to push to the ADC.
//So we need to take the channel number (three bits) and do a bitwise OR on it with 11000;
//This gives us a leading 1 start bit and a 1 for the single-ended read. Then the three following bits
//are the channel selection on the ADC.
TX = Channel; //0000 0XXX
TX |= 0x18; //Start bit + single/diff bit. 0001 1XXX
//We only need to send 5 bits so we are going to left shift the bits 3 spaces to the front of the byte.
TX <<= 3; //11XX X000
gpioWrite(_CS, PI_HIGH);//Start with CS high so we can latch the clock.
gpioWrite(_Clock, PI_LOW);//start clock on the low side.
gpioWrite(_CS, PI_LOW);//Bring CS low now to start bit banging the ADC.
//Now we need to send those bits to the ADC.
for(int i = 0; i < 5; i++)
{
//TX starts with a 1? Do a bitwise AND to see if it matches the mask.
if(TX & 0x80)// 0x80 is 1000 0000
gpioWrite(_MOSI, PI_HIGH);
else
gpioWrite(_MOSI, PI_LOW);
//now shift the bits in the TX byte left 1;
TX <<= 1;
//bounce the clock and start again...
gpioWrite(_Clock, PI_HIGH);
gpioDelay(200);
gpioWrite(_Clock, PI_LOW);
}
//Now we need to read in one empty bit, one null bit, and the 10 bits with our value.
for(int i = 0; i < 12; i++)
{
gpioWrite(_Clock, PI_HIGH);
gpioDelay(200);
gpioWrite(_Clock, PI_LOW);
RX <<= 1; //Shift our bit over so when we read the MISO we can OR the value with RX.
if(gpioRead(_MISO))
RX |= 0x1;
}
gpioWrite(_CS, PI_HIGH); //Bring the CS pin back high to end our transaction.
_Mutex.unlock();
//right shift the first bit because it's the null one. What's left will be our value;
RX >>= 1;
return RX;
}
void gpioToggle(int pinno)
{
gpioRead(pinno) ? gpioWrite(pinno, 0) : gpioWrite(pinno, 1);
}
int main(void){
/* ------------- INICIALIZACIONES ------------- */
boardConfig();
gpioConfig(TEC1, GPIO_INPUT);
gpioConfig(TEC2, GPIO_INPUT);
gpioConfig(TEC3, GPIO_INPUT);
gpioConfig(TEC4, GPIO_INPUT);
gpioConfig(LED1, GPIO_OUTPUT);
gpioConfig(LED2, GPIO_OUTPUT);
gpioConfig(LED3, GPIO_OUTPUT);
bool_t led1 = false;
bool_t led2 = false;
bool_t led3 = false;
int delayTime1 = 250;
int delayTime2 = 250;
int delayTime3 = 250;
delay_t delay1;
delay_t delay2;
delay_t delay3;
delayConfig( &delay1, delayTime1 );
delayConfig( &delay2, delayTime2 );
delayConfig( &delay3, delayTime3 );
while(1) {
if (delayRead(&delay1)) {
gpioWrite( LED1, led1 );
led1 = ! led1;
delayWrite(&delay1,delayTime1);
}
if (delayRead(&delay2)) {
gpioWrite( LED2, led2 );
led2 = ! led2;
delayWrite(&delay2,delayTime2);
}
if (delayRead(&delay3)) {
gpioWrite( LED3, led3 );
led3 = ! led3;
delayWrite(&delay3,delayTime3);
}
if (! gpioRead(TEC1)) {
delayTime1 = 250;
delayTime2 = 250;
delayTime3 = 250;
}
if (! gpioRead(TEC2)) {
delayTime1 = 250;
delayTime2 = 500;
delayTime3 = 750;
}
if (! gpioRead(TEC3)) {
delayTime1 = 750;
delayTime2 = 500;
delayTime3 = 250;
}
if (! gpioRead(TEC4)) {
delayTime1 = 1000;
delayTime2 = 1000;
delayTime3 = 1000;
}
}
return 0 ;
}
/*******************************************************************************
*
* initFetPreDriver
*
* Reset and initialize the Freescale 33937A FET Pre-driver chip
*
*
******************************************************************************/
static int initFetPreDriver(int timer, int spiFd)
{
int retVal = !ERROR;
uint8_t cmd[8];
spiWriteRead_t spiTxRx = {
.out = cmd,
.in = cmd,
.len = 1,
};
int i;
printf("Initializing FET pre driver...\n");
/* Put the chip into reset and disable */
gpioClear(FET_DRIVER_RESET_PORT, FET_DRIVER_RESET_PIN);
gpioClear(FET_DRIVER_ENABLE_PORT, FET_DRIVER_ENABLE_PIN);
taskDelay(100);
/* Take chip out of reset and wait for VDD and VLS to stabilize */
gpioSet(FET_DRIVER_RESET_PORT, FET_DRIVER_RESET_PIN);
taskDelay(50);
/* Clear interrupt status flags */
cmd[0] = CLINT0_CMD | 0xf;
ioctl(spiFd, IO_IOCTL_SPI_WRITE_READ, (int) &spiTxRx);
cmd[0] = CLINT1_CMD | 0xf;
ioctl(spiFd, IO_IOCTL_SPI_WRITE_READ, (int) &spiTxRx);
/* Subscribe to interrupt sources */
cmd[0] = MASK0_CMD | MASK0_OVERTEMP_BIT | MASK0_OVER_CURRENT_BIT
| MASK0_VLS_UNDER_VOLTAGE_BIT ;
ioctl(spiFd, IO_IOCTL_SPI_WRITE_READ, (int) &spiTxRx);
cmd[0] = MASK1_CMD | MASK1_FRAMING_ERROR_BIT
| MASK1_WRITE_ERROR_BIT | MASK1_RESET_EVENT_BIT;
ioctl(spiFd, IO_IOCTL_SPI_WRITE_READ, (int) &spiTxRx);
#if 1
/* Use zero deadtime at pre driver (ftm uses deadtime already) */
cmd[0] = DEADTIME_CMD;
printf("read %d \n", ioctl(spiFd, IO_IOCTL_SPI_WRITE_READ, (int) &spiTxRx));
printf("deadtime returned %x \n", cmd[0]);
#endif
/* Using default deadtime. Otherwise set it here. */
#if 0
/* TODO Test this */
cmd[0] = DEADTIME_CMD | DEADTIME_CALIBRATE_BIT;
write(spiFd, cmd, 1);
taskDelay(10);
read(spiFd, cmd, 1);
ioctl(spiFd, IO_IOCTL_SPI_SET_OPTS, SPI_OPTS_MASTER | SPI_OPTS_PCS_CONT);
taskDelay(PERIOD_16_TIMES_LONGER_THAN_DESIRED_DEADTIME);
ioctl(spiFd, IO_IOCTL_SPI_SET_OPTS, SPI_OPTS_MASTER);
#endif
/* Setup any special mode settings and lock mode */
cmd[0] = MODE_CMD | MODE_FULLON_BIT | MODE_DESATURATION_FAULT_BIT;
//cmd[0] = MODE_CMD | MODE_DESATURATION_FAULT_BIT;
//| MODE_MODE_LOCK_BIT;
ioctl(spiFd, IO_IOCTL_SPI_WRITE_READ, (int) &spiTxRx);
/* Bring up the enable lines (they are tied together in this design)
* and get ready for some f#cking smoke! :) */
gpioSet(FET_DRIVER_ENABLE_PORT, FET_DRIVER_ENABLE_PIN);
cmd[0] = NULL_CMD | 0;
write(spiFd, cmd, 1);
taskDelay(50);
/* Check status for fun */
for (i = 0; i < MAX_NULL_CMD_REG; i++) {
cmd[0] = NULL_CMD | i;
write(spiFd, cmd, 1);
taskDelay(50);
read(spiFd, cmd, 1);
// if (i) {
if (i==0) {
cmd[0] &= ~STATUS_REG0_RESET_EVENT;
if (cmd[0]) {
retVal = ERROR;
}
}
printf("FET Pre Amp Status[%d] %x\n", i, cmd[0]);
// }
}
// read(spiFd, cmd, 1);
// printf("FET Pre Amp Status[%d] %x\n", i, cmd[0]);
taskDelay(50);
if (retVal != ERROR) {
/* Turn on all low sides */
ftmSetOutputMask(FTM_0, MASK_HIGH_SIDE, TRUE);
taskDelay(50);
/* Turn off all low sides */
ftmSetOutputMask(FTM_0, MASK_ALL_OUTPUTS, TRUE);
taskDelay(50);
/* Turn on all high sides */
ftmSetOutputMask(FTM_0, MASK_LOW_SIDE, TRUE);
taskDelay(1000);
/* Turn off all high sides */
ftmSetOutputMask(FTM_0, MASK_ALL_OUTPUTS, TRUE);
/* Good to go! */
printf("FET predriver initialized. \n");
#if 0
if (gpioRead(HALL_A_PORT, HALL_A_PIN)) {
value |= HALL_A_BIT;
}
if (gpioRead(HALL_B_PORT, HALL_B_PIN)) {
value |= HALL_B_BIT;
}
if (gpioRead(HALL_C_PORT, HALL_C_PIN)) {
value |= HALL_C_BIT;
}
if (value > 0 && value <= MAX_HALL_PHASE) {
int idx;
commutator.ready = TRUE;
commutator.hallPosition = value;
idx = commutationStepFromHallPos[commutator.hallPosition];
idx = 2;
// ftmSetOutputMask(FTM_0, commutator.commutationMask[idx], FALSE);
// ftmSetInvCtrl(FTM_0, commutator.bipolarCompliment[idx], TRUE);
} else {
/* Motor Position Fault! */
printf("Motor Position Fault! %d \n", value);
gpioClear(FET_DRIVER_ENABLE_PORT, FET_DRIVER_ENABLE_PIN);
gpioClear(FET_DRIVER_RESET_PORT, FET_DRIVER_RESET_PIN);
updateFlags |= UPDATE_HALT;
}
#endif
} else {
printf("Fault on FET PreDriver! \n");
// gpioClear(FET_DRIVER_ENABLE_PORT, FET_DRIVER_ENABLE_PIN);
// gpioClear(FET_DRIVER_RESET_PORT, FET_DRIVER_RESET_PIN);
// updateFlags |= UPDATE_HALT;
}
return retVal;
}
static void setClock(void)
{
/* -------- 100 MHz (external clock) -----------
* Configure the Multipurpose Clock Generator output to use the external
* clock locked with a PLL at the maximum frequency of 100MHZ
*
* For PLL, the dividers must be set first.
*
* System: 100 MHz
* Bus: 50 MHz
* Flexbus: 25 MHz
* Flash: 25 MHz
*/
clockSetDividers(DIVIDE_BY_1, DIVIDE_BY_2, DIVIDE_BY_4, DIVIDE_BY_4);
clockConfigMcgOut(MCG_PLL_EXTERNAL_100MHZ);
return;
}
errlHndl_t gpioPerformOp(DeviceFW::OperationType i_opType,
TARGETING::Target * i_target,
void * io_buffer,
size_t & i_buflen,
int64_t i_accessType,
va_list i_args)
{
errlHndl_t err = NULL;
gpioAddr_t gpioInfo;
gpioInfo.deviceType = va_arg( i_args, uint64_t );
gpioInfo.portAddr = va_arg( i_args, uint64_t );
TRACDCOMP(g_trac_gpio, ENTER_MRK"gpioPerformOp(): "
"optype %d deviceType %d portAddr %d",
i_opType, gpioInfo.deviceType, gpioInfo.portAddr);
do
{
err = gpioReadAttributes (i_target, gpioInfo);
if( err )
{
break;
}
TARGETING::TargetService& ts = TARGETING::targetService();
TARGETING::Target * i2c_master = ts.toTarget(gpioInfo.i2cMasterPath);
if( i2c_master == NULL )
{
TRACFCOMP( g_trac_gpio,ERR_MRK"gpioPerformOp() - "
"I2C Target not found. Device type %d.",
gpioInfo.deviceType );
/*@
* @errortype
* @reasoncode GPIO_I2C_TARGET_NOT_FOUND
* @severity ERRORLOG::ERRL_SEV_UNRECOVERABLE
* @moduleid GPIO_PERFORM_OP
* @userdata1 Device type
* @userdata2 HUID of target
* @devdesc Invalid GPIO device type
* @custdesc A problem occurred during the IPL
* of the system.
*/
err = new ERRORLOG::ErrlEntry(ERRORLOG::ERRL_SEV_UNRECOVERABLE,
GPIO::GPIO_PERFORM_OP,
GPIO::GPIO_I2C_TARGET_NOT_FOUND,
gpioInfo.deviceType,
TARGETING::get_huid(i_target),
true /*Add HB SW Callout*/ );
err->collectTrace( GPIO_COMP_NAME );
break;
}
if( i_opType == DeviceFW::READ )
{
err = gpioRead(i2c_master,
io_buffer,
i_buflen,
gpioInfo);
if( err )
{
break;
}
}
else if (i_opType == DeviceFW::WRITE )
{
err = gpioWrite(i2c_master,
io_buffer,
i_buflen,
gpioInfo);
if( err )
{
break;
}
}
else
{
TRACFCOMP( g_trac_gpio,ERR_MRK"gpioPerformOp() - "
"Invalid OP type %d.",
i_opType );
/*@
* @errortype
* @reasoncode GPIO_INVALID_OP
* @severity ERRORLOG::ERRL_SEV_UNRECOVERABLE
* @moduleid GPIO_PERFORM_OP
* @userdata1 OP type
* @userdata2 HUID of target
* @devdesc Invalid GPIO device type
* @custdesc A problem occurred during the IPL
* of the system.
*/
err = new ERRORLOG::ErrlEntry(ERRORLOG::ERRL_SEV_UNRECOVERABLE,
GPIO::GPIO_PERFORM_OP,
GPIO::GPIO_INVALID_OP,
i_opType,
TARGETING::get_huid(i_target),
true /*Add HB SW Callout*/ );
err->collectTrace( GPIO_COMP_NAME );
break;
}
} while (0);
return err;
}