void __encoder_read(SENSOR* sensor){
ENCODER_TYPE ticks;
ENCODER* encoder = (ENCODER*)sensor;
TIMER_SNAPSHOT lastTime;
TIMER_SNAPSHOT prevTime;
// Disable interrupts whilst reading counter.
CRITICAL_SECTION{
ticks = encoder->counter;
if(encoder->interpolate){
lastTime = encoder->snapshot[encoder->tickIndex];
prevTime = encoder->snapshot[encoder->tickIndex ^ 1];
}
}
if(encoder->interpolate){
TICK_COUNT lastTick,prevTick,now;
now = clockGetus();
lastTick = timerSnapshotToTicks(&lastTime);
prevTick = timerSnapshotToTicks(&prevTime);
encoder->timeSinceLastTick = now - lastTick; // us since last tick
encoder->durationOfLastTick = lastTick - prevTick; // us between last two ticks (ie speed)
}
if(encoder->inverted){
ticks *= -1;
}
// Store the last read value
encoder->value=ticks;
}
uint8_t marqueeSendByte(MARQUEE* marquee, uint8_t byte){
marquee_init(marquee);
if(marquee->txt){
if(byte=='\n'){
// Start writing at the beginning of the line
marquee->txt[marquee->writePos] = '\0';
marquee->writePos = 0;
CRITICAL_SECTION_START;
if(!marquee->active){
marquee->active = TRUE;
marquee->blink = FALSE;
marquee->readPos=0;
scheduleJob(&marqueeUpdate,(SchedulerData)marquee,clockGetus(),marquee->delayChar);
}
CRITICAL_SECTION_END;
}else if(byte!='\r'){
// Now put the character to the buffer
if(marquee->writePos < marquee->txtSize){
char* put = marquee->txt + marquee->writePos;
CRITICAL_SECTION_START;
*put++ = byte;
*put = '\0';
marquee->writePos += 1;
marquee->readPos = 0;
CRITICAL_SECTION_END;
}
}
}
return byte;
}
// Update the PID with the actual position and return the new output
float pidSetActual(PID* pid,float actual){
// Find time difference since last call
TICK_COUNT dt;
if(g_heartbeat){
// We have got a timer
TICK_COUNT now = clockGetus();
dt = now - pid->lastTime;
pid->lastTime = now;
}else{
// There is no timer - so assume called every 10ms
dt = (TICK_COUNT)10000;
}
float target;
CRITICAL_SECTION{
target = pid->setPoint;
}
float error = target - actual; //find error
//if circular control system, check for and handle wrap around
if (pid->isCircular){
float spanDiv2 = pid->inSpan / 2;
if (error < -spanDiv2){
error += pid->inSpan;
}
if (error > spanDiv2){
error -= pid->inSpan;
}
}
pid->prevError = error; // save error for next go around
//sum the errors for integral and clamp to limit
pid->integral += error;
pid->integral = CLAMP( pid->integral, -(pid->iLimit), pid->iLimit);
//find slope of error over time
float derivative = (error - pid->prevError) / dt;
float out = (pid->kP * error) + (pid->kI * pid->integral) + (pid->kD * derivative);
//compare output to limits
out = CLAMP(out, pid->outMin, pid->outMax);
return out;
}
// Initialise the software
TICK_COUNT appInitSoftware(TICK_COUNT loopStart){
for (int i=0; i < 14; i+=1){
led[i].r = 0;
led[i].g = 0;
led[i].b = 0;
}
setLeds();
halfperiods[0] = 500000; // us
halfperiods[1] = 625000;
switchtime = clockGetus();
newbrightness = minbright;
return 0;
}
uint8_t marqueeSendByte(MARQUEE* marquee, uint8_t byte){
marquee_init(marquee);
if(marquee->txt){
if(byte=='\n'){
// Start writing at the beginning of the line
marquee->txt[marquee->writePos] = '\0';
marquee->writePos = 0;
CRITICAL_SECTION{
if(!marquee->active){
marquee->active = TRUE;
marquee->blink = FALSE;
marquee->readPos=0;
scheduleJob(&marqueeUpdate,(SchedulerData)marquee,clockGetus(),marquee->delayChar);
}
}
}else if(byte!='\r'){
// Start running a new animation
void gaitRunnerPlay(G8_RUNNER* runner, uint8_t animation, int16_t loopSpeed, DRIVE_SPEED speed, int16_t repeatCount){
// Update variables with interrupts off - in case the gait is
// updated under interrupts
TICK_COUNT now = clockGetus();
CRITICAL_SECTION {
runner->animation = animation;
runner->repeatCount = repeatCount;
runner->frame = 0;
runner->playing = TRUE;
runner->startTime = now;
runner->currentTime = (speed<0) ? loopSpeed : 0;
runner->totalTime = loopSpeed;
runner->speed = speed;
runner->backwards = FALSE;
}
// Set servos to initial position
gaitRunnerProcess(runner);
}
// Update the gait runner and move servos to new positions
// Call it from your main loop or via the scheduler to do it in the background
// NB There is no point scheduling any faster than 20ms as that is the servo refresh rate
// Return true if an animation is playing
boolean gaitRunnerProcess(G8_RUNNER* runner){
if(!gaitRunnerIsPlaying(runner) || runner->speeds==null){
return FALSE;
}
TICK_COUNT now = clockGetus();
int16_t interval = (now - runner->startTime)>>16;
if(interval == 0){
return TRUE;
}
// There has been a noticeable change in time
runner->startTime = now;
if(runner->backwards){
interval *= -1;
}
interval *= runner->speed;
// Re-check as drive speed could be zero
if(interval == 0){
return TRUE;
}
// Locate the current animation
const G8_ANIMATION* animation = &runner->animations[runner->animation];
// Update the current time with the new interval
int16_t currentTime = runner->currentTime + interval;
if(currentTime >= runner->totalTime){
// We have finished playing the animation
if(pgm_read_byte(&animation->sweep)==FALSE){
currentTime %= runner->totalTime; // Set back to start of loop
if(runner->repeatCount){
runner->repeatCount -= 1; // One less frame to go
if(runner->repeatCount==0){
runner->playing = FALSE; // we have reached the end
currentTime = 0; // set servos to final position
}
}
}else{
// Start going backwards through the animation
currentTime = runner->totalTime - (currentTime - runner->totalTime);
runner->backwards = TRUE;
}
}else if(currentTime < 0){
// We have moved before the start
if(pgm_read_byte(&animation->sweep)==FALSE){
currentTime = runner->totalTime + currentTime;
if(runner->repeatCount){
runner->repeatCount += 1; // One more frame to go
if(runner->repeatCount==0){
runner->playing = FALSE; // we have reached the end
currentTime = 0; // set servos to start position
}
}
}else{
// We have completed a sweep
runner->backwards = FALSE;
currentTime = -currentTime;
if(runner->repeatCount){
runner->repeatCount -= 1; // One less frame to go
if(runner->repeatCount==0){
runner->playing = FALSE; // we have reached the end
currentTime = 0; // set servos to initial position
}
}
}
}
runner->currentTime = currentTime; // range is 0....totalTime
// Current time in the range 0...SCALE_X
uint16_t frameTime = interpolateU(currentTime, 0,runner->totalTime, 0, SCALE_X);
uint16_t frameStartTime = 0;
uint16_t frameEndTime = SCALE_X;
// Locate the correct frame
const G8_FRAME* frame = (const G8_FRAME*)pgm_read_word(&animation->frames);
uint8_t i;
for(i = pgm_read_byte(&animation->numFrames)-1; i>0; i--){
const G8_FRAME* f = &frame[i];
frameStartTime = pgm_read_word(&f->time);
if(frameStartTime <= frameTime){
frame = f;
break;
}
frameEndTime = frameStartTime;
frameStartTime = 0;
}
runner->frame = i;
#ifdef DEBUG
rprintf("\n%u,%d",i,currentTime);
#endif
// Now have:- frameStartTime <= frameTime <= frameEndTime
// We now need to find the distance along the curve (0...1) that represents
// the x value = frameTime
// First guess from 0..1
uint16_t frameTimeOffset = frameTime-frameStartTime;
double distanceGuess = ((double)(frameTimeOffset)) / ((double)(frameEndTime-frameStartTime));
const G8_LIMB_POSITION* limb = (const G8_LIMB_POSITION*)pgm_read_word(&frame->limbs);
for(uint16_t l = 0; l < runner->num_actuators; l++, limb++){
double distanceMin = 0.0;
double distanceMax = 1.0;
double distance = distanceGuess;
// Find the correct distance along the line for the required frameTime
for(uint8_t iterations=0; iterations<20; iterations++){
uint16_t actualX = calcX(limb, distance);
if(actualX == frameTimeOffset) break; // Found it
if( actualX < frameTimeOffset){
// We need to increase t
distanceMin = distance;
}else{
distanceMax = distance;
}
// Next guess is half way between
distance = distanceMin + ((distanceMax - distanceMin) / 2);
}
// We now know the distance
runner->speeds[l] = calcY(limb,distance);
#ifdef DEBUG
rprintf(",%d",speed);
#endif
} // next limb
#ifndef DEBUG
// Set all the servo speeds in quick succession
for(uint16_t l = 0; l < runner->num_actuators; l++){
__ACTUATOR* servo = (__ACTUATOR*)pgm_read_word(&runner->actuators[l]);
int16_t speed = (int16_t)(runner->speeds[l]) + (int16_t)(runner->delta[l]);
speed = CLAMP(speed,DRIVE_SPEED_MIN,DRIVE_SPEED_MAX);
__act_setSpeed(servo,(DRIVE_SPEED)speed);
}
#endif
return gaitRunnerIsPlaying(runner);
}
// This is the main loop
TICK_COUNT appControl(LOOP_COUNT loopCount, TICK_COUNT loopStart) {
//Toggle mode via button
if (!buttonWait){
if(SWITCH_pressed(&button)){
// pressed
mode = (mode + 1) % NUMMODES;
buttonWait = 1;
buttonWaitStartTime = loopStart;
// Clear vals for new mode
for (int i=set; i < 14; i++){
led[i].r = 0;
led[i].g = 0;
led[i].b = 0;
}
setLeds();
}
}
// Wait 1 second after noticing a button press before allowing another button press
else {
if (clockHasElapsedGetOverflow(buttonWaitStartTime, buttonWaitTime, &unusedButtonRemainingTime)){
buttonWait = 0;
}
}
/// alternating half bright half fading
if (mode == 1) {
if (clockHasElapsedGetOverflow(switchtime, halfperiods[set], &remainingtime)) {
// rprintf("boom!\n");
delay_ms(400);
// Switch modes if we've reached the end of increasing brightness
if (dir == 1){
set = (set + 1) % 2;
}
switchtime = clockGetus();
dir *= -1;
remainingtime = halfperiods[set] - remainingtime;
// Set half the LEDs to maximum brightness
for (int i=(1-set); i < 14; i+=2){
// led[i].b = maxbright * 2;
led[i].r = maxbright * 2;
led[i].g = maxbright;
}
}
// Calculate brightness of dimmer LEDs
if(dir == 1){ // increasing brightness
newbrightness = maxbright - (maxbright - minbright) * remainingtime / halfperiods[set];
}
else{ // decreasing brightness
newbrightness = minbright + (maxbright - minbright) * remainingtime / halfperiods[set];
}
// Monocolor
// newbrightness *= 2;
// newbrightness += 1;
for (int i=set; i < 14; i+=2){
// blue
// led[i].b = newbrightness;
// orange
led[i].r = newbrightness * 2 + 1;
led[i].g = newbrightness + 1;
}
setLeds();
}
///
else if (mode == 2) {
if (clockHasElapsedGetOverflow(switchtime, halfperiods[set], &remainingtime)) {
// rprintf("boom!\n");
set = (set + 1) % 2;
switchtime = clockGetus();
remainingtime = halfperiods[set] - remainingtime;
}
for (int i=0; i < 14; i+=2){
led[i].b = 0;
}
j = (14 * (remainingtime /1000)) / (halfperiods[set] / 1000);
led[j].b = maxbright;
setLeds();
}
/// moving orange
else if (mode == 3) {
if (clockHasElapsedGetOverflow(switchtime, halfperiods[set], &remainingtime)) {
//delay_ms(250);
// Switch modes if we've reached the end of increasing brightness
//if (dir == 1){
set = (set + 1) % 2;
//}
switchtime = clockGetus();
//dir *= -1;
remainingtime = halfperiods[set] - remainingtime;
}
for (int i=0; i < 14; i++){
led[i].r = 0;
led[i].g = 0;
led[i].b = 0;
}
j = (14 * (remainingtime /1000)) / (halfperiods[set] / 1000);
led[j].r = maxbright;
led[j].g = maxbright/2;
//led[j].b = maxbright;
setLeds();
}
/// FLASHING WHITE
else if (mode == 4) {
if (clockHasElapsedGetOverflow(switchtime, halfperiods[set], &remainingtime)) {
delay_ms(400);
// Switch modes if we've reached the end of increasing brightness
if (dir == 1){
set = (set + 1) % 2;
}
switchtime = clockGetus();
dir *= -1;
remainingtime = halfperiods[set] - remainingtime;
}
// Calculate brightness
if(dir == 1){ // increasing brightness
newbrightness = maxbright - (maxbright - minbright) * remainingtime / halfperiods[set];
}
else{ // decreasing brightness
newbrightness = minbright + (maxbright - minbright) * remainingtime / halfperiods[set];
}
for (int i=0; i < 14; i++){
led[i].r = newbrightness * 2;
led[i].g = newbrightness * 2;
led[i].b = newbrightness * 2;
}
setLeds();
}
else if (mode == 5)
{
k = (k+1)%w;
y = 4;
z = 7;
w = 7;
int val = (k*y)%z;
// >>> for k in xrange(w): print (k*y)%z;
// for (int k=0; k < w; k+=x)
// {
// val = (k*y)%z;
// }
for (int i=0; i < 14; i++){
led[i].r = maxbright * 2;
led[i].g = maxbright;
// led[i].b = newbrightness * 2;
}
led[val].r = maxbright * 2;
led[val].g = maxbright * 2;
led[val+7].r = maxbright * 3;
led[val+7].g = maxbright * 2;
setLeds();
return 500000;
}
// ///////
// // BlinkM control (doesn't work; probably need pull up resistors)
// uint8_t blinkm_packet_setcolor[4];
// blinkm_packet_setcolor[0] = (uint8_t)'n';
// blinkm_packet_setcolor[1] = 50;
// blinkm_packet_setcolor[2] = 50;
// blinkm_packet_setcolor[3] = 50;
// i2cMasterSend(&blinkm.i2cInfo, sizeof(blinkm_packet_setcolor), blinkm_packet_setcolor);
// // i2cMasterSend(&blinkm.i2cInfo, sizeof(blinkm_packet_setcolor), *blinkm_packet_setcolor);
// delay_ms(50);
else if (mode == 0){
for (int i=0; i < 14; i++){
led[i].r = 0;
led[i].g = 0;
led[i].b = 0;
}
setLeds();
}
return 20000;
}
// This routine is called repeatedly - its your main loop
TICK_COUNT appControl(LOOP_COUNT loopCount, TICK_COUNT loopStart){
TICK_COUNT frameEnd = frameStart + 20000;
wdt_reset();
// very occasionally RF module misses interrupt.
// this usually happens when signal is weak.
// testing interrupt pin manually here is an effective workaround.
//if (pin_is_low(E7)){
// cyrfInterupt(&cyrf_0);
//}
//cyrf6936_RX_mode(&cyrf_0);
// poll sensors as often as possible
// get readings from A2D.
//int tmp = a2dConvert8bit(ADC_CH_ADC6);
// = ;
currDraw += a2dConvert10bit(ADC_CH_ADC1);
rcAccelX += a2dConvert10bit(ADC_CH_ADC7);
rcAccelY += a2dConvert10bit(ADC_CH_ADC6);
rcAccelZ += a2dConvert10bit(ADC_CH_ADC5);
rcGyroX += a2dConvert10bit(ADC_CH_ADC4);
rcGyroY += a2dConvert10bit(ADC_CH_ADC3);
sensorCount++; // keep track of number of times ADC has been read.
int tmp;
if(frameEnd <= clockGetus()) {
frameStart = frameEnd;
// this section happens every 20ms frame.
// frame counter.
data[RX_FRA_NUM]++;
data[RX_BAT_CUR] = (currDraw / sensorCount) >>2;
// average accelerometer.
RAcc1.RXACC = (rcAccelX / sensorCount) -AccOffset;
RAcc1.RYACC = (rcAccelY / sensorCount) -AccOffset;
RAcc1.RZACC = (rcAccelZ / sensorCount) -AccOffset;
RAccSize1 = sqrt((RAcc1.RXACC*RAcc1.RXACC)+(RAcc1.RYACC*RAcc1.RYACC)+(RAcc1.RZACC*RAcc1.RZACC));
// extract roll and pitch from accelerometer's 3 axis.
data[RX_ACCEL_Y] = AtAcc.AXZ = 90*atan2(RAcc1.RXACC,RAcc1.RZACC) + data[RX_AUTOP_Y_TRIM] - 0x7F;
data[RX_ACCEL_Y] += 0x7F;
data[RX_ACCEL_X] = AtAcc.AYZ = 90*atan2(RAcc1.RYACC,RAcc1.RZACC) + data[RX_AUTOP_X_TRIM] - 0x7F;
data[RX_ACCEL_X] += 0x7F;
// *** experimental ***
// multiply accelerometer readings by a factor of current draw.
data[RX_ACCEL_Y] = AtAcc.AXZ = AtAcc.AXZ - data[RX_BAT_CUR]/4.5;
data[RX_ACCEL_Y] += 0x7F;
data[RX_ACCEL_X] = AtAcc.AYZ = AtAcc.AYZ - data[RX_BAT_CUR]/5.5;
data[RX_ACCEL_X] += 0x7F;
// average gyro.
int x_gyro_offset = data[RX_GYRO_X_OFFSET_L] + (0x100*data[RX_GYRO_X_OFFSET_L]);
int y_gyro_offset = data[RX_GYRO_Y_OFFSET_L] + (0x100*data[RX_GYRO_Y_OFFSET_L]);
data[RX_GYRO_X] = AtGyr.AYZ = (rcGyroX / sensorCount);
data[RX_GYRO_X] += 0x7F - x_gyro_offset;
data[RX_GYRO_Y] = AtGyr.AXZ = (rcGyroY / sensorCount);
data[RX_GYRO_Y] += 0x7F - y_gyro_offset;
// work out estimated angle based on previous estimate and sensor data.
data[RX_EST_X] = AtEst1.AYZ = (((AtEst0.AYZ + ((AtGyr.AYZ - x_gyro_offset)*GyroAmp))*GyroBias) + (AtAcc.AYZ)) / (1 + GyroBias);
data[RX_EST_X] += 0x7F;
data[RX_EST_Y] = AtEst1.AXZ = (((AtEst0.AXZ + ((AtGyr.AXZ - y_gyro_offset)*GyroAmp))*GyroBias) + (AtAcc.AXZ)) / (1 + GyroBias);
data[RX_EST_Y] += 0x7F;
// something mental is going on at power on so stop excessive values.
if(AtEst1.AYZ > (0x2A7FF + 90)){ AtEst1.AYZ = (0x2A7FF + 90);}
if(AtEst1.AYZ < 0x2A7FF - 90){ AtEst1.AYZ = (0x2A7FF - 90);}
if(AtEst1.AXZ > (0x38DFF + 90)){ AtEst1.AXZ = (0x38DFF + 90);}
if(AtEst1.AXZ < 0x38DFF - 90){ AtEst1.AXZ = (0x38DFF - 90);}
//data[RX_EST_X] = data[RX_ACCEL_X];
//data[RX_EST_Y] = data[RX_ACCEL_Y];
//rprintfu16((int)(AtGyr.AXZ/0x10000) + 0x8000);
//rprintfu16(AtGyr.AXZ );
//rprintfProgStrM("\n");
currDraw = rcAccelX = rcAccelY = rcAccelZ = rcGyroX = rcGyroY = 0;
AtEst0 = AtEst1;
sensorCount1=sensorCount;
sensorCount=0;
/* // read battery and current sensor
tmp = a2dConvert8bit(ADC_CH_ADC0);
if(tmp > data[RX_BAT_VOLT]){
data[RX_BAT_VOLT]++;
}
else if(tmp < data[RX_BAT_VOLT]){
data[RX_BAT_VOLT]--;
}
data[RX_BAT_CUR] = a2dConvert8bit(ADC_CH_ADC1);
*/
if(!(testIfPacketReceived(data)==0)){
// this section done if a packet has been received this frame.
// (the data[] should have arrived during an interrupt on the RF module IRQ pin.)
uptime++;
// set servo positions.
set_servos(data);
pin_high(C1); // warning LED off
// watch for PS2 controller button presses.
// these are de-bounced by running a simple counter.
uint16_t tmp = ((data[TX_PS2_0] <<8) + data[TX_PS2_1]);
uint8_t i;
for (i=0; i<16; i++){
if(tmp & (1<<i)){
// button pressed. increase counter
//rprintfProgStrM("\nPressed:"); rprintfu08(i);
if (buttonPressed[i] < 0xFF){ buttonPressed[i]++; }
}
else if(buttonPressed[i] > 0){
// button has just been released. save value in counter to de-bounce buttons.
//rprintfProgStrM("\nReleased:"); rprintfu08(i);
buttonReleased[i]=buttonPressed[i];
buttonPressed[i]=0;
}
else{
buttonReleased[i]=0;
buttonPressed[i]=0;
}
}
if (buttonReleased[PS2_B_BTN_TRIANGLE] > 5){
set_trims(data);
}
if (buttonReleased[PS2_B_BTN_CIRCLE] > 5){
reset_trims();
}
if (buttonReleased[PS2_B_BTN_X] > 5){
data[RX_GYRO_X_OFFSET_H] = (uint8_t)(AtGyr.AYZ / 0x100);
data[RX_GYRO_X_OFFSET_L] = (uint8_t)(AtGyr.AYZ - (AtGyr.AYZ*0x100));
data[RX_GYRO_Y_OFFSET_H] = (uint8_t)(AtGyr.AXZ / 0x100);
data[RX_GYRO_Y_OFFSET_L] = (uint8_t)(AtGyr.AXZ - (AtGyr.AXZ*0x100));
eeprom_write_byte(&data_eeprom[RX_GYRO_X_OFFSET_H], data[RX_GYRO_X_OFFSET_H]);
eeprom_write_byte(&data_eeprom[RX_GYRO_X_OFFSET_L], data[RX_GYRO_X_OFFSET_L]);
eeprom_write_byte(&data_eeprom[RX_GYRO_Y_OFFSET_H], data[RX_GYRO_Y_OFFSET_H]);
eeprom_write_byte(&data_eeprom[RX_GYRO_Y_OFFSET_L], data[RX_GYRO_Y_OFFSET_L]);
}
if (buttonReleased[PS2_B_DPAD_UP] > 5){
if (buttonPressed[PS2_B_BTN_R1] > 0){
if(buttonPressed[PS2_B_BTN_R2] > 0){
//increase autopilot sensitivity (Y axis).
data[RX_AUTOP_Y_MUL] ++;
}
else{
//increase autopilot trim (Y axis).
data[RX_AUTOP_Y_TRIM] ++;
}
}
else if (data[RX_SERVO_2_MUL] < 137){
data[RX_SERVO_2_MUL]++;
}
}
if (buttonReleased[PS2_B_DPAD_DOWN] > 5){
if (buttonPressed[PS2_B_BTN_R1] > 0){
if(buttonPressed[PS2_B_BTN_R2] > 0){
if (data[RX_AUTOP_Y_MUL] > 2){
//decrease aoutopilot sensitivity (Y axis).
data[RX_AUTOP_Y_MUL] --;
}
}
else{
//decrease autopilot trim (Y axis).
data[RX_AUTOP_Y_TRIM] --;
}
}
else if (data[RX_SERVO_2_MUL] > 117){
data[RX_SERVO_2_MUL]--;
}
}
if (buttonReleased[PS2_B_DPAD_RIGHT] > 5){
if (buttonPressed[PS2_B_BTN_R1] > 0){
if(buttonPressed[PS2_B_BTN_R2] > 0){
//increase aoutopilot sensitivity (X axis).
data[RX_AUTOP_X_MUL] ++;
}
else{
//increase autopilot trim (X axis).
data[RX_AUTOP_X_TRIM] ++;
}
}
else if (data[RX_SERVO_4_MUL] < 137){
data[RX_SERVO_4_MUL]++;
data[RX_SERVO_3_MUL] = data[RX_SERVO_4_MUL];
}
}
if (buttonReleased[PS2_B_DPAD_LEFT] > 5){
if (buttonPressed[PS2_B_BTN_R1] > 0){
if(buttonPressed[PS2_B_BTN_R2] > 0){
if (data[RX_AUTOP_X_MUL] > 2){
//decrease autopilot sensitivity (X axis).
data[RX_AUTOP_X_MUL] --;
}
}
else{
//decrease autopilot trim (X axis).
data[RX_AUTOP_X_TRIM] --;
}
}
else if (data[RX_SERVO_4_MUL] > 117){
data[RX_SERVO_4_MUL]--;
data[RX_SERVO_3_MUL] = data[RX_SERVO_4_MUL];
}
}
}
else{
// no packet received this frame
pin_low(C1); // warning LED on
}
// enable receive mode.
cyrf6936_RX_mode(&cyrf_0);
cyrf6936_RX_mode(&cyrf_1);
if(data[RX_PAC_GAP] > 25){
// no data received for 1/2 second
// so clear PS2 controller readings
// and put servos to default positions.
if(data[TX_CONTROLER]==0){
data[TX_PS2_LY] = 128;
}
else{
data[TX_PS2_LY] = 254;
}
data[TX_PS2_LX] = 128;
data[TX_PS2_RY] = 128;
data[TX_PS2_RX] = 128;
set_servos(data);
//pin_low(C0); // warning LED on
}
else{
//pin_high(C0); // warning LED off
}
// debug status LED off
pin_high(C0);
}
// This routine is called once to allow you to set up any other variables in your program
// You can use 'clock' function here.
// The loopStart parameter has the current clock value in μS
TICK_COUNT appInitSoftware(TICK_COUNT loopStart){
act_setSpeed(&servo_0,DRIVE_SPEED_CENTER);
act_setSpeed(&servo_1,DRIVE_SPEED_CENTER);
act_setSpeed(&servo_2,DRIVE_SPEED_CENTER);
act_setSpeed(&servo_3,DRIVE_SPEED_CENTER);
act_setSpeed(&servo_4,DRIVE_SPEED_CENTER);
act_setSpeed(&servo_5,DRIVE_SPEED_CENTER);
// starting values for trims.
data[RX_SERVO_0_CH] = eeprom_read_byte(&data_eeprom[RX_SERVO_0_CH]);
data[RX_SERVO_0_CL] = eeprom_read_byte(&data_eeprom[RX_SERVO_0_CL]);
data[RX_SERVO_1_CH] = eeprom_read_byte(&data_eeprom[RX_SERVO_1_CH]);
data[RX_SERVO_1_CL] = eeprom_read_byte(&data_eeprom[RX_SERVO_1_CL]);
data[RX_SERVO_2_CH] = eeprom_read_byte(&data_eeprom[RX_SERVO_2_CH]);
data[RX_SERVO_2_CL] = eeprom_read_byte(&data_eeprom[RX_SERVO_2_CL]);
data[RX_SERVO_3_CH] = eeprom_read_byte(&data_eeprom[RX_SERVO_3_CH]);
data[RX_SERVO_3_CL] = eeprom_read_byte(&data_eeprom[RX_SERVO_3_CL]);
data[RX_SERVO_4_CH] = eeprom_read_byte(&data_eeprom[RX_SERVO_4_CH]);
data[RX_SERVO_4_CL] = eeprom_read_byte(&data_eeprom[RX_SERVO_4_CL]);
data[RX_SERVO_0_MUL] = eeprom_read_byte(&data_eeprom[RX_SERVO_0_MUL]);
data[RX_SERVO_1_MUL] = eeprom_read_byte(&data_eeprom[RX_SERVO_1_MUL]);
data[RX_SERVO_2_MUL] = eeprom_read_byte(&data_eeprom[RX_SERVO_2_MUL]);
data[RX_SERVO_3_MUL] = eeprom_read_byte(&data_eeprom[RX_SERVO_3_MUL]);
data[RX_SERVO_4_MUL] = eeprom_read_byte(&data_eeprom[RX_SERVO_4_MUL]);
data[RX_AUTOP_X_MUL] = eeprom_read_byte(&data_eeprom[RX_AUTOP_X_MUL]);
data[RX_AUTOP_Y_MUL] = eeprom_read_byte(&data_eeprom[RX_AUTOP_Y_MUL]);
data[RX_AUTOP_X_TRIM] = eeprom_read_byte(&data_eeprom[RX_AUTOP_X_TRIM]);
data[RX_AUTOP_Y_TRIM] = eeprom_read_byte(&data_eeprom[RX_AUTOP_Y_TRIM]);
data[RX_GYRO_X_OFFSET_L] = eeprom_read_byte(&data_eeprom[RX_GYRO_X_OFFSET_L]);
data[RX_GYRO_X_OFFSET_H] = eeprom_read_byte(&data_eeprom[RX_GYRO_X_OFFSET_H]);
data[RX_GYRO_Y_OFFSET_L] = eeprom_read_byte(&data_eeprom[RX_GYRO_Y_OFFSET_L]);
data[RX_GYRO_Y_OFFSET_H] = eeprom_read_byte(&data_eeprom[RX_GYRO_Y_OFFSET_H]);
if ((data[RX_SERVO_1_CH] < 0x60) | (data[RX_SERVO_1_CH] > 0x80)){
reset_trims();
}
if ((data[RX_SERVO_3_MUL] > 137) || (data[RX_SERVO_3_MUL] < 117)){ data[RX_SERVO_3_MUL] = 127;}
if ((data[RX_SERVO_4_MUL] > 137) || (data[RX_SERVO_4_MUL] < 117)){ data[RX_SERVO_4_MUL] = 127;}
// initialise RF module.
cyrf6936_Initialise_soft(&cyrf_0);
cyrf6936_Initialise_soft(&cyrf_1);
// enable interrupt on pin connected to cyrf6936 interrupt pin.
cyrfIntEnable();
frameStart=clockGetus();
data[RX_BAT_VOLT] = a2dConvert8bit(ADC_CH_ADC0);
AtEst1.AYZ = AtEst0.AYZ = 0x2A7FF;
AtEst1.AXZ = AtEst1.AXZ = 0x38DFF;
return 0; // don't pause after
}
