/*****************************************************************************
* Function Name : test_motor
* Description : Turns on a specified motor for a specified period of time
* and duty cycle
* Parameters : type - The type field of the motor test packet
* status - Status field of the motor test packet (not yet used)
* length - The length of the payload data array
* data - data[0] = motor number
* data[1:2] = on time (milli secs)
* data[3:4] = duty cycle (percent)
* Return Value : success indicator - 0 for failed, 1 for succeeded
*****************************************************************************/
unsigned char test_motor(unsigned char type, unsigned char status, \
unsigned char length, unsigned char* data)
{ unsigned int motor_id;
int on_time;
int dutycycle;
char ack_string[40]="motor OK\n";
MacPacket packet;
Payload pld;
motor_id = (unsigned int) data[0];
on_time = (unsigned long)( (data[3] << 8) + data[2]);
dutycycle = (int)((data[5] << 8) + data[4]);
tiHSetDC(motor_id, dutycycle);
swatchDelayMs(on_time);
tiHSetDC(motor_id, 0);
// send an ack packet back - could have data later...
// Get a new packet from the pool
packet = radioRequestPacket(sizeof(ack_string));
if(packet == NULL) return 0;
//macSetDestAddr(packet, RADIO_DEST_ADDR);
// Prepare the payload
pld = packet->payload;
paySetStatus(pld, STATUS_UNUSED);
paySetType(pld, type);
// Read message data into the payload
memcpy(payGetData(pld), & ack_string, sizeof(ack_string)); // copy ack_string to packet
// Enqueue the packet for broadcast
radioEnqueueTxPacket(packet);
return 1; //success
}
void tiHSetup(void) {
//This setup is unique to PWM_IPCLK_SCALE1
//If the clock scaler is changed, this MUST be changed too!
pwm_period = (int)((float)FCY/((float)PWM_FREQ * 1.0))- 1;
sevtcmp_pwm = (int)(ADC_TRIG_POINT * (float)pwm_period);
max_pwm = 2*sevtcmp_pwm;
tiHSetupPeripheral();
int i;
for(i=0; i< NUM_PWM; i++) {
outputs[i].throt_f = 0.0;
outputs[i].throt_i = 0;
outputs[i].mode = TIH_MODE_COAST;
outputs[i].dir = TIH_FWD;
}
//Start all channels at 0 throttle in Forward / Coast mode
tiHSetDC(1,0);
tiHSetDC(2,0);
tiHSetDC(3,0);
tiHSetDC(4,0);
}
// set PWM values for short duration for each motor
// throttle[0], throttle[1], duration
void cmdSetThrust(unsigned char type, unsigned char status, unsigned char length, unsigned char *frame)
{ int thrust1 = frame[0] + (frame[1] << 8);
int thrust2 = frame[2] + (frame[3] << 8);
unsigned int run_time_ms = frame[4] + (frame[5] << 8);
DisableIntT1; // since PID interrupt overwrites PWM values
// wiring was scrambled Jan. 9, 2013 on RSF velociRoACH robot
tiHSetDC(1, -thrust2);
tiHSetDC(2, thrust1);
delay_ms(run_time_ms);
tiHSetDC(1,0);
tiHSetDC(2,0);
EnableIntT1;
}
unsigned char cmdSetThrustOpenLoop(unsigned char type, unsigned char status, unsigned char length, unsigned char *frame) {
int thrust1 = frame[0] + (frame[1] << 8);
int thrust2 = frame[2] + (frame[3] << 8);
unsigned int run_time_ms = frame[4] + (frame[5] << 8);
DisableIntT1; // since PID interrupt overwrites PWM values
tiHSetDC(1, thrust1);
tiHSetDC(2, thrust2);
delay_ms(run_time_ms);
tiHSetDC(1,0);
tiHSetDC(2,0);
EnableIntT1;
return 1;
}
void pidSetControl()
{ int j;
// 0 = right side
for(j=0; j < NUM_PIDS; j++)
{ //pidobjs[0] : right side
// p_input has scaled velocity interpolation to make smoother
// p_state is [16].[16]
pidObjs[j].p_error = pidObjs[j].p_input + pidObjs[j].interpolate - pidObjs[j].p_state;
pidObjs[j].v_error = pidObjs[j].v_input - pidObjs[j].v_state; // v_input should be revs/sec
//Update values
UpdatePID(&(pidObjs[j]));
} // end of for(j)
if(pidObjs[0].onoff && pidObjs[1].onoff) // both motors on to run
{
tiHSetDC(1, pidObjs[0].output);
tiHSetDC(2, pidObjs[1].output);
}
else // turn off motors if PID loop is off
{ tiHSetDC(1,0); tiHSetDC(2,0); }
}
