void setMode(void){ if( ~PIND & SW_BUTTON ){ // printf( "PIND is OFF\n"); if( SwitchSts == 0 ){ SwitchSts = 1; } }else{ // printf( "PIND is ON\n"); if( SwitchSts == 1 ){ SwitchSts = 0; mMode++; if( (LED_MAX - mMode) <= 0 ){ PORTC = LED_BAT|LED_TxD|LED_RxD|LED_AUX|LED_MANAGE|LED_PROGRAM; mMode = MODE_0; }else{ PORTC = ~(1 << (LED_MAX - mMode)); } stopMotion(); ServoControl( 0 ); modeCounter = 1; if( mMode == MODE_1 ){ modeWait = mode1act[0][0]; } } } }
void servoSummer(uint16_t*pid_period_ptr, float* imp_servo_ptr ) { *pid_period_ptr=0; //turn off pid calculations if ( (our_time[DAYOFWEEK]==2 || our_time[DAYOFWEEK]==5) && our_time[HOURE]==8 && (our_time[MINUTES]<10) ) { *imp_servo_ptr = ServoControl(100,servo_period_G); } //open for 10minutes else { *imp_servo_ptr = ServoControl(-100,servo_period_G); //signal to servo to close } }
void moveTest(void){ if(isMoving() == 0 ){ int *motion = motionList[motionNumber]; int max = motion[0]; if( motionCount > max ){ walkCounter++; if (walkCounter < 10) { printf("### moveTest 1\n"); motionNumber = ACT_TURN_LEFT; } else if (walkCounter < 20) { printf("### moveTest 2\n"); motionNumber = ACT_TURN_RIGHT; } else if (walkCounter < 30) { printf("### moveTest 3\n"); motionNumber = ACT_WALK2; } else { walkCounter=0; } motionCount = 1; } ServoControl( motion[motionCount] ); motionCount++; } }
int main(void){ int log = 0; //Start Switch // DDRA = 0x00; // PORTA = 0x12; //Start PORT A for switch and IR sensors DDRA = 0xFC; PORTA = 0xFE; //LED Initial DDRC = 0x7F; PORTC = 0x7E; DDRD = 0x70; PORTD = 0x11; MotorInit(); initSerial(); char * readData = NULL; int isFinish = 0; sensorInit(); if (isCaptureMode ==1) dxl_write_byte( BROADCAST_ID, P_TORQUE_ENABLE, 0 ); while(1){ sensorTest(0); sensorTest(1); sensorTest(2); setMode(); if( checkSerialRead() > 0 ){ readData = getReadBuffer(); if( readData != NULL ){ // printf( "readData=%s\n", &readData[0] ); split( &readData[0] ); switch( serCmd[0] ){ case EVT_ACTION: ServoControl( serCmd[1] ); // setSpeedTest( serCmd[1] ); sendAck(1); break; case EVT_START_MOTION: startMotion( serCmd[1], serCmd[2] ); PORTC = ~(1 << (LED_MAX - 2)); sendAck(1); break; case EVT_STOP_MOTION: stopMotion(); sendAck(1); break; case EVT_FORCE_MOTION: forceMotion( serCmd[1], serCmd[2] ); break; case EVT_GET_NOW_ANGLE: getAngle(); break; case EVT_SET_ANGLE: setAngle(); case EVT_GET_ACT_ANGLE: if( serCmd[1] >= ACT_MAX ){ sendAck(0); }else{ sendActAngle(serCmd[1]); } break; case EVT_GET_LOAD: getLoad(); // printf( "%d\n", movingTime ); break; case EVT_GET_VOLTAGE: getVoltage(); break; case EVT_TORQUE_DISABLE: dxl_write_byte( BROADCAST_ID, P_TORQUE_ENABLE, 0 ); break; case EVT_WATCH_DOG: watchDogCnt = 0; break; case EVT_MOTION_EDIT: break; case 999: // printf( "finish\n"); sendAck(999); isFinish = 1; break; default: sendAck(0); } if( isFinish > 0 ){ MotorControl( 0, 0 ); break; } memset( readData, 0x00, SERIAL_BUFFER_SIZE ); } } memset( &serCmd[0], 0x00, sizeof(int) * SERIAL_BUFFER_SIZE ); if (~PINA & SW_START ) { if (log == 1) printf( "main() 0\n"); if( iStart > 0 ){ iStart = 0; PORTC = LED_BAT|LED_TxD|LED_RxD|LED_AUX|LED_MANAGE|LED_PROGRAM|LED_PLAY; if (isCaptureMode != 1) ServoControl( 0 ); } }else{ if( iStart == 0 ){ PORTC &= ~LED_PLAY; iStart = 1; } if( modeWait <= 0 ){ if (log == 1) printf( "main() 1\n"); setModeAction(); if (isMovetest == 1) { moveTest(); } else { move(); } }else{ if (log == 1) printf( "main() 2\n"); modeWait -= MAIN_DELAY; } } if (sensorValue[0] == 0 && sensorValueOld[0] != sensorValue[0]) { if (log == 1) printf( "### main() sensorValue[0] == 0\n"); PORTC |= LED_PROGRAM; //edit }else if (sensorValueOld[0] != sensorValue[0]){ if (log == 1) printf( "### main() sensorValue[0] == 1\n"); PORTC &= ~LED_PROGRAM; //edit } if (sensorValue[1] == 0 && sensorValueOld[1] != sensorValue[1]) { if (log == 1) printf( "### main() sensorValue[1] == 0\n"); PORTC |= LED_MANAGE; //mon }else if (sensorValueOld[1] != sensorValue[1]){ if (log == 1) printf( "### main() sensorValue[1] == 1\n"); PORTC &= ~LED_MANAGE; //mon } if (sensorValue[2] == 0 && sensorValueOld[2] != sensorValue[2]) { if (log == 1) printf( "### main() sensorValue[2] == 0\n"); PORTC |= LED_AUX; //AUX }else if (sensorValueOld[2] != sensorValue[2]){ if (log == 1) printf( "### main() sensorValue[2] == 1\n"); PORTC &= ~LED_AUX; //AUX } sensorValueOld[0] = sensorValue[0]; sensorValueOld[1] = sensorValue[1]; sensorValueOld[2] = sensorValue[2]; // walk pattern LED // brinkLED(); _delay_ms(MAIN_DELAY); watchDogCnt++; caputureCount1++; if (caputureCount1 == 25){ if (isCaptureMode == 1) getAngle(); caputureCount1 = 0; } } }
void move(void){ int log = 0; if( motionTimes > 0 && isMoving() == 0 ){ if (motionNumber >= ACT_TYPE_MAX) { printf("move Oops! motionNumber:%d\n", motionNumber); // motionNumber = ACT_WALK2; } int *motion = motionList[motionNumber]; // printf("move motionNumber:%d, motion = %d\n", motionNumber, *motion); int max = motion[0]; // printf("move motionCount:%d, max:%d\n", motionCount, max); if( motionCount > max ){ if (log == 1) printf("move 1\n"); /* if (motionNumber != ACT_WALK2) { printf("### move 1\n"); motionNumber = ACT_WALK2; }else{ if (sensorValue[2] == 1) { printf("### move 2\n"); motionNumber = ACT_TURN_LEFT; walkCounter = 0; } else if (sensorValue[0] == 1 && sensorValue[1] == 1) { printf("### move 3\n"); motionNumber = ACT_WALK2; walkCounter = 0; } else if (sensorValue[0] == 1 && sensorValue[1] == 0) { printf("### move 4\n"); // motionNumber = ACT_TURN_LEFT; walkCounter = 0; } else if (sensorValue[0] == 0 && sensorValue[1] == 1) { printf("### move 5\n"); motionNumber = ACT_TURN_RIGHT; walkCounter = 0; } else if (++walkCounter > 5) { printf("### move 6\n"); motionNumber = ACT_TURN_LEFT; walkCounter = 0; } } */ // printf("### motionCount > max. motionCount:%d, max:%d\n", motionCount, max); // printf("#%d,%d,%d,%d,%d,%d;\n", diffmaxTest[0],diffmaxTest[1],diffmaxTest[2],diffmaxTest[3],diffmaxTest[4],diffmaxTest[5] ); motionCount = 1; if( motionTimes < 99 && --motionTimes <= 0 ){ if (log == 1) printf("move 2\n"); /* if( nextMotionTimes > 0 ){ motionNumber = nextMotionNumber; motionTimes = nextMotionTimes; nextMotionTimes = 0; _delay_ms(300); }else{ stopMotion(); } */ return; } } if (log == 1) printf("move 3 motionNumber:%d\n", motionNumber); if (motionNumber == ACT_NECK_SEARCH_SWING) { searchSensors[motionCount] = sensorTest(0); printf("move 4 searchSensors[%d]:%d\n", motionCount, searchSensors[motionCount]); } // printf("### motionCount:%d\n", motionCount); ServoControl( motion[motionCount] ); motionCount++; } }
// 核心进程 void CoreControl(void) { INT16U i = 0, j; INT8U tpls = 0; // INT16U tmp; // 串口发送的状态, 和串口字 //INT8U sciState = 0;//, sciChar; StartSpeeder(); ClearDistanceCounter(); MotorControlInit(); crsN = 0; nowLoop = 0; FOREVER() { //PORTB = ~PORTB; // 判断小按键是否按下, 是则进入菜单 if (!PTIP_PTIP0) { WaitEnable(); Wait(20); if (!PTIP_PTIP0) { StopRun(); StartMenu(); WaitSmallButtonPress(); Wait(1500); } } // 采集红外值 for (i = 0;i < nIR / 2;i++) { irSendDouble(irRevPair[i][0], irRevPair[i][1]); for (j = 0;j < 100;j++); ReadADCDouble(irRevPair[i][2],irRevPair[i][3], &wir[irRevPair[i][0]], &wir[irRevPair[i][1]]); } // 将各值归一化 for (i = 0;i < nIR;i++) { if (whiteAvg[i] < wir[i]) { irValue[i] = 100; } else if (blackAvg[i] > wir[i]) { irValue[i] = 0; } else { irValue[i] = (100 * (wir[i] - blackAvg[i])) / ( whiteAvg[i] - blackAvg[i]); } } /** 红外值的手工修正 **/ //irValue[2] = irValue[2] * 100 / 94; //irValue[3] = irValue[3] * 100 / 86; //irValue[6] = irValue[6] * 100 / 94; // 找最低 minIRv = 100; maxIRv = 0; ttotal = 0; for (i = 0;i < nIR;i++) { if ( minIRv > irValue[i] ) { minIRn2 = minIRn; minIRn = i; minIRv2 = minIRv; minIRv = irValue[i]; } if ( maxIRv < irValue[i] ) { maxIRv = irValue[i]; } ttotal+= irValue[i]; } total = ttotal; // 求出精确位置 if ( minIRn <= (nIR - 1 - 1) && minIRn >= 1) { position = ir_position[minIRn - 1] + irValue[minIRn - 1] * IR_SPACE_BETWEEN / (irValue[minIRn - 1] + irValue[minIRn + 1]); } else if (minIRn == 0) { if (irValue[1] >= 90) { position = ir_position[0] - (IR_SPACE_BETWEEN / 2) * irValue[0] / (irValue[0] + irValue[1]); } else { position = ir_position[0] + (IR_SPACE_BETWEEN / 2) * irValue[0] / (irValue[0] + irValue[1]); } } else { if (irValue[nIR - 1 - 1] >= 92/** TODO 这样合适么? **/) { position = ir_position[nIR - 1] + (IR_SPACE_BETWEEN / 2) * irValue[nIR - 1] / (irValue[nIR - 1 - 1] + irValue[nIR - 1]); } else { position = ir_position[nIR - 1] - (IR_SPACE_BETWEEN / 2) * irValue[nIR - 1] / (irValue[nIR - 1 - 1] + irValue[nIR - 1]); } } // 排错, 理论上前后两次之间的值相差不应超过15 if ( ((position > last_position)?(position - last_position):(last_position - position)) > 60) { if (nowLoop == 1) { if (GetDistance() - lastAllWhiteDist > 50) position = last_position; } else if (nowLoop == 2) { if (GetDistance() - (StartLineDist[1] - StartLineDist[0]) - lastAllWhiteDist > 50) position = last_position; } } // 类模糊法判断各种路面情况 /** TODO 各各变量的权值还可以调节(通过数据), 变量数还可以增加 **/ //情况 minIRv minIRv2 maxIRv total blStateArr[NORMAL] = (100 - minIRv) + (100 - minIRv2) + maxIRv + total / nIR; blStateArr[LOST] = minIRv + minIRv2 + maxIRv + (100 - total / nIR); blStateArr[CROSS] = (100 - minIRv) + (100 - minIRv2) + (100 - maxIRv) + (100 - total / nIR); blStateArr[START] = (100 - minIRv) + (100 - minIRv2) + maxIRv + (100 - total / nIR); // 找最有可能的情况 blState = NORMAL; for (i = 1;i < 4;i++) { if (blStateArr[blState] < blStateArr[i]) { blState = i; } } tBlState = blState; // 根据判断结果作出相应 if ( blState != NORMAL) { ProcessSpecialPoint(); if (GetDistance() - lastAllWhiteDist < 50) { last_position = position; } else { position = last_position; } } else { last_position = position; } // 得出舵机转角 #if nIR == 8 // 与以前的匹配 if (position > 125) { position = 125 + (position - 125) * 220 / 175; } else { position = 125 - (125 - position) * 220 / 175; } #endif /** TODO 使用一个函数发生器来为舵机提供转角 **/ servoTgtAngle = (INT8U)PosToAgl(position); //累加舵机角度 if (PerDistSrvTotal < 0xFFFFFF && PerDistSrvN < 0xFFF0 && servoTgtAngle < 130 && servoTgtAngle > 50 ) { PerDistSrvTotal += servoTgtAngle; PerDistSrvN++; } // 简单转速控制 tgtSpeed = (INT16S)(minSpeed + (maxSpeed - minSpeed) * (speed_arr[position] ) / 40); if (tgtSpeed < 50 || tgtSpeed > XSpeed) { tgtSpeed = lastTgtSpeed; } else { lastTgtSpeed = tgtSpeed; } // 转动舵机 ServoControl(); // 转动马达, 有限制距离的 if (DistLimit != 0 && DistLimit * 25 < GetDistance() ) { StopRun(); } else { MotorControl(); } tdist = GetDistance(); /*if(SCI0SR1 & 0x80) { switch(sciState) { case 0: sciChar = 0xFE; break; case 1: sciChar = (INT8U)position; break; case 2: sciChar = (INT8U)servoTgtAngle; break; case 3: sciChar = ((INT8U)((3927 * 4) / GetSpeed()) * _RTI_P )& 0xFF; break; case 4: sciChar = (INT8U)(GetDistance()); default: break; } sciState = (INT16U)(sciState + 1) % 5; SCI0DRL = sciChar; } */ /////////////////////////////////////////////////////////////////////// } }
int main() { int offset[DOF]; int dor[DOF]; if (setInitPos(offset, dor)) return 1; std::vector<ServoControl> s; s.push_back(ServoControl(p15)); //left foot role s.push_back(ServoControl(p16)); s.push_back(ServoControl(p17)); s.push_back(ServoControl(p18)); s.push_back(ServoControl(p19)); s.push_back(ServoControl(p20)); s.push_back(ServoControl(p26)); //right foot role s.push_back(ServoControl(p25)); s.push_back(ServoControl(p24)); s.push_back(ServoControl(p23)); s.push_back(ServoControl(p22)); s.push_back(ServoControl(p21)); for (int i=0; i<DOF; i++) s[i].setPos(offset[i]); for (int i=0; i<DOF; i++) s[i].setPower(1); std::vector<int> motionframe; char fname[] = "/local/motion.txt"; if (readMotion(fname, &motionframe)) return 1; // int fnum = motionframe.size()/13; // motion_interpolation(s,motionframe,offset,dor,50); motion_interpolation_infinit(s,motionframe, offset, dor, 20); /* L=1; for (int j=0; j<DOF; j++) { s[j].setPos(offset[j]); } wait_ms(5000); */ for (int i=0; i<DOF; i++) s[i].setPower(0); return 0; }