- msg.input[0] = 0; // unused, Hardware starts at 1
- for (int i=1; i<=8; i++) {
- msg.input[i] = (transfer_area->E_Main & (1<<(i-1))) >> (i-1);
+ pthread_mutex_lock(&pwm_mutex);
+ msg.header.stamp = ros::Time::now();
+ sem_wait(&hFt->lock);
+ for (int i=0; i<=7; i++) {
+ msg.input[i] = (transfer_area->E_Main & (1<<i)) >> i;
+ }
+ for (int i=0; i<=7; i++) {
+ msg.output[i] = pwm[i];
+ pwm[i] = pwm_next[i];
+
+ if (pwm[i] == 0) {
+ transfer_area->M_Main &= ~(1<<(i));
+ } else {
+ transfer_area->M_Main |= (1<<(i));
+ }
+ transfer_area->MPWM_Main[i] = pwm[i];