/*-----------------------------------------------------------------------------
* 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
}
}
