/*----------------------------------------------------------------------------- * restore pwm output state * * restore block: * byte 0: 0b0000xxxx: on/off mask: 0 off, 1 on , bit0 .. channel 0 * bit1 .. channel 1 * bit2 .. channel 2 * bit3 .. channel 3 * byte 1: low byte channel 0 * byte 2: high byte channel 0 * byte 3: low byte channel 1 * byte 4: high byte channel 1 * byte 5: low byte channel 2 * byte 6: high byte channel 2 * byte 7: low byte channel 3 * byte 8: high byte channel 3 */ static void RestorePwm(void) { uint8_t *ptrToEeprom; uint8_t flags; uint8_t sizeRestore = 1 + NUM_PWM_CHANNEL * sizeof(uint16_t); uint8_t pwmLow; uint8_t pwmHigh; uint8_t pwmMask; uint8_t i; /* find the newest state */ for (ptrToEeprom = EEPROM_PWM_RESTORE_START; ptrToEeprom < (EEPROM_PWM_RESTORE_END - sizeRestore); ptrToEeprom += sizeRestore) { pwmMask = eeprom_read_byte(ptrToEeprom); if (pwmMask != 0xff) { break; } } if (ptrToEeprom > (EEPROM_PWM_RESTORE_END - sizeRestore)) { /* not found -> no restore * set pwm[] = 0xffff, state off */ for (i = 0; i < NUM_PWM_CHANNEL; i++) { PwmOn(i, false); PwmSet(i, 0xffff); } spNextPtrToEeprom = EEPROM_PWM_RESTORE_START; return; } spNextPtrToEeprom = ptrToEeprom + sizeRestore; if (spNextPtrToEeprom > (EEPROM_PWM_RESTORE_END - sizeRestore)) { spNextPtrToEeprom = EEPROM_PWM_RESTORE_START; } /* restore pwm output */ flags = DISABLE_INT; for (i = 0; i < NUM_PWM_CHANNEL; i++) { if (pwmMask & (1 << i)) { PwmOn(i, true); } pwmLow = eeprom_read_byte(ptrToEeprom + 1 + i * sizeof(uint16_t)); pwmHigh = eeprom_read_byte(ptrToEeprom + 1 + i * sizeof(uint16_t) + 1); PwmSet(i, pwmLow + 256 * pwmHigh); } /* delete */ eeprom_write_byte((uint8_t *)ptrToEeprom, 0xff); for (i = 0; i < NUM_PWM_CHANNEL; i++) { eeprom_write_byte(ptrToEeprom + 1 + i * 2, 0xff); eeprom_write_byte(ptrToEeprom + 1 + i * 2 + 1, 0xff); } RESTORE_INT(flags); }
/*----------------------------------------------------------------------------- * init */ void PwmInit(void) { /* timer 1: use oc1a, oc1b, oc1c */ OCR1A = 0; OCR1B = 0; OCR1C = 0; /* fast pwm mode 10 bit, * clear output on compare match, set on top, * clock prescaler 8 -> @1.8432MHz: 1843200 / 1024 / 8 = 225 Hz pwm */ TCCR1A = (1 << COM1A1) | (1 << COM1B1) | (1 << COM1C1) | (1 << WGM11) | (1 << WGM10); TCCR1B = (1 << WGM12) | (1 << CS11); /* timer 4: use oc4a */ TC4H = 0; OCR4A = 0; /* count top 1023 */ TC4H = 0x3; OCR4C = 0xff; /* fast pwm mode * clear output on compare match, set on top, * clock prescaler 8 -> @1.8432MHz: 1843200 / 1024 / 8 = 225 Hz pwm */ TCCR4A = (1 << COM4A1) | (1 << PWM4A); TCCR4B = (1 << CS42); PwmSet(0, 0); PwmOn(0, false); PwmSet(1, 0); PwmOn(1, false); PwmSet(2, 0); PwmOn(2, false); PwmSet(3, 0); PwmOn(3, false); }
/** * Motor functions */ static void MotorSet(int8_t left, int8_t right) { if (left >= 0) { PwmSet( &pwm_left_fwd, MOTOR_PWM_T, left); PwmSet( &pwm_left_rev, MOTOR_PWM_T, 0); } else { PwmSet( &pwm_left_fwd, MOTOR_PWM_T, 0); PwmSet( &pwm_left_rev, MOTOR_PWM_T, -left); } if (right >= 0) { PwmSet( &pwm_right_fwd, MOTOR_PWM_T, right); PwmSet( &pwm_right_rev, MOTOR_PWM_T, 0); } else { PwmSet( &pwm_right_fwd, MOTOR_PWM_T, 0); PwmSet( &pwm_right_rev, MOTOR_PWM_T, -right); } }
/*----------------------------------------------------------------------------- * process bus telegrams */ static void ProcessBus(uint8_t ret) { TBusMsgType msgType; uint8_t i; bool msgForMe = false; uint8_t state; uint8_t mask8; uint8_t action; TBusDevRespInfo *pInfo; TBusDevRespActualValue *pActVal; TClient *pClient; static TBusTelegram sTxMsg; static bool sTxRetry = false; bool flag; uint8_t val8; if (sTxRetry) { sTxRetry = BusSend(&sTxMsg) != BUS_SEND_OK; return; } if (ret == BUS_MSG_OK) { msgType = spBusMsg->type; switch (msgType) { case eBusDevReqReboot: case eBusDevReqInfo: case eBusDevReqActualValue: case eBusDevReqSetValue: case eBusDevReqSwitchState: case eBusDevReqSetAddr: case eBusDevReqEepromRead: case eBusDevReqEepromWrite: case eBusDevReqSetClientAddr: case eBusDevReqGetClientAddr: case eBusDevRespActualValueEvent: if (spBusMsg->msg.devBus.receiverAddr == MY_ADDR) { msgForMe = true; } break; case eBusButtonPressed1: case eBusButtonPressed2: case eBusButtonPressed1_2: msgForMe = true; break; default: break; } } else if (ret == BUS_MSG_ERROR) { ButtonTimeStampRefresh(); } if (msgForMe == false) { return; } switch (msgType) { case eBusDevReqReboot: /* use watchdog to reboot */ /* set the watchdog timeout as short as possible (14 ms) */ cli(); wdt_enable(WDTO_15MS); /* wait for reset */ while (1); break; case eBusButtonPressed1: ButtonEvent(spBusMsg->senderAddr, 1); break; case eBusButtonPressed2: ButtonEvent(spBusMsg->senderAddr, 2); break; case eBusButtonPressed1_2: ButtonEvent(spBusMsg->senderAddr, 1); ButtonEvent(spBusMsg->senderAddr, 2); break; case eBusDevReqInfo: /* response packet */ pInfo = &sTxMsg.msg.devBus.x.devResp.info; sTxMsg.type = eBusDevRespInfo; sTxMsg.senderAddr = MY_ADDR; sTxMsg.msg.devBus.receiverAddr = spBusMsg->senderAddr; pInfo->devType = eBusDevTypePwm4; strncpy((char *)(pInfo->version), ApplicationVersion(), sizeof(pInfo->version)); pInfo->version[sizeof(pInfo->version) - 1] = '\0'; sTxRetry = BusSend(&sTxMsg) != BUS_SEND_OK; break; case eBusDevReqActualValue: /* response packet */ pActVal = &sTxMsg.msg.devBus.x.devResp.actualValue; sTxMsg.type = eBusDevRespActualValue; sTxMsg.senderAddr = MY_ADDR; sTxMsg.msg.devBus.receiverAddr = spBusMsg->senderAddr; pActVal->devType = eBusDevTypePwm4; PwmGetAll(pActVal->actualValue.pwm4.pwm, sizeof(pActVal->actualValue.pwm4.pwm)); val8 = 0; for (i = 0; i < NUM_PWM_CHANNEL; i++) { PwmIsOn(i, &flag); val8 |= flag ? 1 << i: 0; } pActVal->actualValue.pwm4.state = val8; sTxRetry = BusSend(&sTxMsg) != BUS_SEND_OK; break; case eBusDevReqSetValue: if (spBusMsg->msg.devBus.x.devReq.setValue.devType != eBusDevTypePwm4) { break; } mask8 = spBusMsg->msg.devBus.x.devReq.setValue.setValue.pwm4.set; for (i = 0; i < NUM_PWM_CHANNEL; i++) { action = (0x3 << (i * 2) & mask8) >> (i * 2); switch (action) { case 0x00: /* no action, ignore pwm[] from telegram */ break; case 0x01: /* set current pwm, ignore pwm[] from telegram */ PwmOn(i, true); break; case 0x02: /* set to pwm[] from telegram */ PwmSet(i, spBusMsg->msg.devBus.x.devReq.setValue.setValue.pwm4.pwm[i]); PwmOn(i, true); break; case 0x03: /* off, ignore pwm[] from telegram */ PwmOn(i, false); break; default: break; } } /* response packet */ sTxMsg.type = eBusDevRespSetValue; sTxMsg.senderAddr = MY_ADDR; sTxMsg.msg.devBus.receiverAddr = spBusMsg->senderAddr; sTxRetry = BusSend(&sTxMsg) != BUS_SEND_OK; break; case eBusDevReqSwitchState: state = spBusMsg->msg.devBus.x.devReq.switchState.switchState; if ((state & 0x01) != 0) { SwitchEvent(spBusMsg->senderAddr, 1, true); } else { SwitchEvent(spBusMsg->senderAddr, 1, false); } if ((state & 0x02) != 0) { SwitchEvent(spBusMsg->senderAddr, 2, true); } else { SwitchEvent(spBusMsg->senderAddr, 2, false); } /* response packet */ sTxMsg.type = eBusDevRespSwitchState; sTxMsg.senderAddr = MY_ADDR; sTxMsg.msg.devBus.receiverAddr = spBusMsg->senderAddr; sTxMsg.msg.devBus.x.devResp.switchState.switchState = state; sTxRetry = BusSend(&sTxMsg) != BUS_SEND_OK; break; case eBusDevReqSetAddr: sTxMsg.senderAddr = MY_ADDR; sTxMsg.type = eBusDevRespSetAddr; sTxMsg.msg.devBus.receiverAddr = spBusMsg->senderAddr; eeprom_write_byte((uint8_t *)MODUL_ADDRESS, spBusMsg->msg.devBus.x.devReq.setAddr.addr); sTxRetry = BusSend(&sTxMsg) != BUS_SEND_OK; break; case eBusDevReqEepromRead: sTxMsg.senderAddr = MY_ADDR; sTxMsg.type = eBusDevRespEepromRead; sTxMsg.msg.devBus.receiverAddr = spBusMsg->senderAddr; sTxMsg.msg.devBus.x.devResp.readEeprom.data = eeprom_read_byte((const uint8_t *)spBusMsg->msg.devBus.x.devReq.readEeprom.addr); sTxRetry = BusSend(&sTxMsg) != BUS_SEND_OK; break; case eBusDevReqEepromWrite: sTxMsg.senderAddr = MY_ADDR; sTxMsg.type = eBusDevRespEepromWrite; sTxMsg.msg.devBus.receiverAddr = spBusMsg->senderAddr; eeprom_write_byte((uint8_t *)spBusMsg->msg.devBus.x.devReq.readEeprom.addr, spBusMsg->msg.devBus.x.devReq.writeEeprom.data); sTxRetry = BusSend(&sTxMsg) != BUS_SEND_OK; break; case eBusDevRespActualValueEvent: pClient = sClient; for (i = 0; i < sNumClients; i++) { if ((pClient->address == spBusMsg->senderAddr) && (pClient->state == eEventWaitForConfirmation)) { TBusDevActualValuePwm4 *p; uint16_t buf[NUM_PWM_CHANNEL]; PwmGetAll(buf, sizeof(buf)); val8 = 0; for (i = 0; i < NUM_PWM_CHANNEL; i++) { PwmIsOn(i, &flag); val8 |= flag ? 1 << i: 0; } p = &spBusMsg->msg.devBus.x.devResp.actualValueEvent.actualValue.pwm4; if ((memcmp(p->pwm, buf, sizeof(buf)) == 0) && (p->state == val8)) { pClient->state = eEventConfirmationOK; } break; } pClient++; } break; case eBusDevReqSetClientAddr: sTxMsg.senderAddr = MY_ADDR; sTxMsg.type = eBusDevRespSetClientAddr; sTxMsg.msg.devBus.receiverAddr = spBusMsg->senderAddr; for (i = 0; i < BUS_MAX_CLIENT_NUM; i++) { uint8_t *p = &spBusMsg->msg.devBus.x.devReq.setClientAddr.clientAddr[i]; eeprom_write_byte((uint8_t *)(CLIENT_ADDRESS_BASE + i), *p); } sTxRetry = BusSend(&sTxMsg) != BUS_SEND_OK; GetClientListFromEeprom(); break; case eBusDevReqGetClientAddr: sTxMsg.senderAddr = MY_ADDR; sTxMsg.type = eBusDevRespGetClientAddr; sTxMsg.msg.devBus.receiverAddr = spBusMsg->senderAddr; for (i = 0; i < BUS_MAX_CLIENT_NUM; i++) { uint8_t *p = &sTxMsg.msg.devBus.x.devResp.getClientAddr.clientAddr[i]; *p = eeprom_read_byte((const uint8_t *)(CLIENT_ADDRESS_BASE + i)); } sTxRetry = BusSend(&sTxMsg) != BUS_SEND_OK; break; default: break; } }
//------------------------------------------------------------------------------ void main(void) { WDTCTL = WDTPW+WDTHOLD; // Stop watchdog timer FCTL2 = FWKEY + FSSEL0 + FN0; // MCLK/2 for Flash Timing Generator read_flash_segA(); Rs232_Init(); // RS232 初始化 115200 P3 TDCM3_Init(); // 電子羅盤 初始化 P4DIR = 0xff; // Set P4. to output direction P4OUT = 0; WDT_flag = true; TBCTL = TBSSEL_2 + MC_1; // SCLK, up-down mode PWM timeClock AD_Init(); WDTCTL = WDT_ARST_1000; //--開狗 CloseMotorPower(); //----開電測試系統 // TimerA 啟動讀秒 TACCTL0 = CCIE; // CCR0 interrupt enabled TACCR0 = 16384-1; // for 0.5 Hz TACTL = TASSEL_1 + MC_1; // ACLK, contmode // _BIS_SR(GIE); Setting_mode = true; long int cou_delay; while(Setting_mode){ //---- setting mode if(Set_OriginX_flag){ Set_OriginX_flag = 0; OpenMotorPower(); cou_delay = (long int)motor_T_AD * 1150; // delay while(cou_delay > 0){ cou_delay--;} DoTheAd(AD_AVGtime,1); CloseMotorPower(); Set_OriginX = (int)(Angle_X*100); write_flash_segA(); } if(Set_OriginY_flag){ Set_OriginY_flag = 0; OpenMotorPower(); cou_delay = (long int)motor_T_AD * 1150; // delay while(cou_delay > 0){ cou_delay--;} DoTheAd(AD_AVGtime,1); CloseMotorPower(); Set_OriginY = (int)(Angle_Y*100); write_flash_segA(); } _BIS_SR(LPM0_bits + GIE); } IE2 &= ~URXIE1 ; // Disable USART1 RX interrupt ReStart_Sec += ((unsigned long int)ReStart_Day * 3600); //-- // ReStart_Sec += ((unsigned long int)ReStart_Day * 60); //-- 每隔分鐘數 PwmSet(); int testRun = 0; Leveling = true; while(1) //-------------- working mode { if(Leveling){ WDTCTL = WDTPW+WDTHOLD; // Stop watchdog timer TACCTL0 &= ~CCIE; // close timer Leveling = false; outPutcount = false; //TDCM3_Enable(); testRun++; sprintf(string,"this is %02d time \r\n",testRun); UART1_SendStr(string); OpenMotorPower(); //----開電 WDTCTL = WDTPW+WDTHOLD; // Stop watchdog timer long int cou = (long int)motor_T_AD * 1150; // 100Hz delay while(cou > 0){ cou--; } GoToRange_V2(); GoHome(); CloseMotorPower(); //----關電 outPutcount = true; TACCTL0 |= CCIE; // open timer } _BIS_SR(LPM0_bits + GIE); // Enter LPM0, Enable interrupts } }