void motorsInit(void) {
float sumPitch, sumRoll, sumYaw;
int i;
AQ_NOTICE("Motors init\n");
memset((void *)&motorsData, 0, sizeof(motorsData));
if (p[MOT_FRAME] > 0.01f && p[MOT_FRAME] < 4.01f) {
AQ_NOTICE("Motors: ERROR! Predefined frame types are no longer supported.\n");
return;
}
motorsData.distribution = (motorsPowerStruct_t *)&p[MOT_PWRD_01_T];
sumPitch = 0.0f;
sumRoll = 0.0f;
sumYaw = 0.0f;
for (i = 0; i < MOTORS_NUM; i++) {
motorsPowerStruct_t *d = &motorsData.distribution[i];
if (d->throttle != 0.0f || d->pitch != 0.0f || d->roll != 0.0f || d->yaw != 0.0f) {
// CAN LO
if (((uint32_t)p[MOT_CANL]) & (1<<i))
motorsCanInit(i);
// CAN HI (PDB)
else if (((uint32_t)p[MOT_CANH]) & (1<<i))
motorsCanInit(i+16);
// PWM
else if (i < PWM_NUM_PORTS)
motorsPwmInit(i);
motorsData.active[i] = 1;
motorsData.activeList[motorsData.numActive++] = i;
sumPitch += d->pitch;
sumRoll += d->roll;
sumYaw += d->yaw;
}
}
if (fabsf(sumPitch) > 0.01f)
AQ_NOTICE("Motors: Warning pitch control imbalance\n");
if (fabsf(sumRoll) > 0.01f)
AQ_NOTICE("Motors: Warning roll control imbalance\n");
if (fabsf(sumYaw) > 0.01f)
AQ_NOTICE("Motors: Warning yaw control imbalance\n");
#ifdef MOTORS_CAN_LOGGING
if (p[MOT_CANL] != 0.0f || p[MOT_CANH] != 0.0f)
motorsSetupLogging();
#endif
motorsSetCanGroup();
motorsOff();
canTelemRegister(motorsReceiveTelem, CAN_TYPE_ESC);
}
int main(void)
{
SYSTEMConfigPerformance(SYS_CLOCK);
enableInterrupts();
initADC();
initTimer2(); //for PWM
initPWM();
initLCD();
initTimer4(); //for pwm
initSW();
int track = 1;
int SpeedR = 0;
int SpeedL = 0;
int ones = 0;
int dec = 0;
float voltage = 0;
//Actual main state machine loop
while(1)
{
Speed = ADC1BUF0;
moveCursorLCD(0,0);
voltage = Speed * ((double)3.3/1023);
ones = floor(voltage);
dec = (voltage - ones) * 10;
printCharLCD('0'+ones);
printCharLCD('.');
printCharLCD('0'+dec);
printCharLCD('V');
delayUs(10000); //10ms - Just to help the LCD not strobe too much
switch(state)
{
case wait_Press: //handled by ISR
break;
case debouncePress:
state = wait_Release;
break;
case wait_Release: //handled by ISR
break;
case debounceRelease:
if (track == 1){
state = forward;
}
if (track == 2){
state = idle2;
}
if (track == 3){
state = reverse;
}
if (track == 4){
state = idle1;
}
break;
case idle1:
track = 1;
motorsOff();
state = wait_Press;
break;
case forward:
track = 2;
if(Speed == 511){
SpeedR = 1023;
SpeedL = 1023;
}
else if(Speed < 511){
SpeedR = 1023;
SpeedL = (2*Speed);
}
else if(Speed > 511){
SpeedR = 2*(1023 - Speed);
SpeedL = 1023;
}
else if (Speed == 1023){
SpeedR = 0;
SpeedL = 1023;
}
else if (Speed == 0){
SpeedR = 1023;
SpeedL = 0;
}
RightMotor_Write(SpeedR);
LeftMotor_Write(SpeedL);
break;
case idle2:
track = 3;
motorsOff();
state = wait_Press;
break;
case reverse:
track = 4;
if(Speed == 511){
SpeedR = 1023;
SpeedL = 1023;
}
else if(Speed < 511){
SpeedR = 1023;
SpeedL = 2*Speed;
}
else if(Speed > 511){
SpeedR = 2*(1023 - Speed);
SpeedL = 1023;
}
else if (Speed == 1023){
SpeedR = 1023;
SpeedL = 0;
}
else if (Speed == 0){
SpeedR = 0;
SpeedL = 1023;
}
RightMotor_Write(0-SpeedR); //maybe it needs the zero to be interpreted as negative?
LeftMotor_Write(0-SpeedL);
break;
}
}
return 0;
}
void turnAround() {
LeftMotor_Write(-850);
RightMotor_Write(850);
delayUs(1500000);
motorsOff();
}
void controlTaskCode(void *unused) {
float yaw;
float throttle;
float ratesDesired[3];
uint16_t overrides[3];
#ifdef USE_QUATOS
float quatDesired[4];
float ratesActual[3];
#else
float pitch, roll;
float pitchCommand, rollCommand, ruddCommand;
#endif // USE_QUATOS
AQ_NOTICE("Control task started\n");
// disable all axes' rate overrides
overrides[0] = 0;
overrides[1] = 0;
overrides[2] = 0;
while (1) {
// wait for work
CoWaitForSingleFlag(imuData.dRateFlag, 0);
// this needs to be done ASAP with the freshest of data
if (supervisorData.state & STATE_ARMED) {
if (RADIO_THROT > p[CTRL_MIN_THROT] || navData.mode > NAV_STATUS_MANUAL) {
supervisorThrottleUp(1);
// are we in altitude hold mode?
if (navData.mode > NAV_STATUS_MANUAL) {
// override throttle with nav's request
throttle = pidUpdate(navData.altSpeedPID, navData.holdSpeedAlt, -VELOCITYD) * MOTORS_SCALE / RADIO_MID_THROTTLE;
// don't allow negative throttle to be built up
if (navData.altSpeedPID->iState < 0.0f) {
navData.altSpeedPID->iState = 0.0f;
}
} else {
throttle = ((uint32_t)RADIO_THROT - p[CTRL_MIN_THROT]) * MOTORS_SCALE / RADIO_MID_THROTTLE * p[CTRL_FACT_THRO];
}
// limit
throttle = constrainInt(throttle, 1, MOTORS_SCALE);
// if motors are not yet running, use this heading as hold heading
if (motorsData.throttle == 0) {
navData.holdHeading = AQ_YAW;
controlData.yaw = navData.holdHeading;
// Reset all PIDs
pidZeroIntegral(controlData.pitchRatePID, 0.0f, 0.0f);
pidZeroIntegral(controlData.rollRatePID, 0.0f, 0.0f);
pidZeroIntegral(controlData.yawRatePID, 0.0f, 0.0f);
pidZeroIntegral(controlData.pitchAnglePID, 0.0f, 0.0f);
pidZeroIntegral(controlData.rollAnglePID, 0.0f, 0.0f);
pidZeroIntegral(controlData.yawAnglePID, 0.0f, 0.0f);
// also set this position as hold position
if (navData.mode == NAV_STATUS_POSHOLD) {
navUkfSetHereAsPositionTarget();
}
}
// constrict nav (only) yaw rates
yaw = compassDifference(controlData.yaw, navData.holdHeading);
yaw = constrainFloat(yaw, -p[CTRL_NAV_YAW_RT]/400.0f, +p[CTRL_NAV_YAW_RT]/400.0f);
controlData.yaw = compassNormalize(controlData.yaw + yaw);
// DVH overrides direct user pitch / roll requests
if (navData.mode != NAV_STATUS_DVH) {
controlData.userPitchTarget = RADIO_PITCH * p[CTRL_FACT_PITC];
controlData.userRollTarget = RADIO_ROLL * p[CTRL_FACT_ROLL];
} else {
controlData.userPitchTarget = 0.0f;
controlData.userRollTarget = 0.0f;
}
// navigation requests
if (navData.mode > NAV_STATUS_ALTHOLD) {
controlData.navPitchTarget = navData.holdTiltN;
controlData.navRollTarget = navData.holdTiltE;
} else {
controlData.navPitchTarget = 0.0f;
controlData.navRollTarget = 0.0f;
}
// manual rate cut through for yaw
if (RADIO_RUDD > p[CTRL_DEAD_BAND] || RADIO_RUDD < -p[CTRL_DEAD_BAND]) {
// fisrt remove dead band
if (RADIO_RUDD > p[CTRL_DEAD_BAND]) {
ratesDesired[2] = (RADIO_RUDD - p[CTRL_DEAD_BAND]);
} else {
ratesDesired[2] = (RADIO_RUDD + p[CTRL_DEAD_BAND]);
}
// calculate desired rate based on full stick scale
ratesDesired[2] = ratesDesired[2] * p[CTRL_MAN_YAW_RT] * DEG_TO_RAD * (1.0f / 700.0f);
// keep up with actual craft heading
controlData.yaw = AQ_YAW;
navData.holdHeading = AQ_YAW;
// request override
overrides[2] = CONTROL_MIN_YAW_OVERRIDE;
} else {
// currently overriding?
if (overrides[2] > 0) {
// request zero rate
ratesDesired[2] = 0.0f;
// follow actual craft heading
controlData.yaw = AQ_YAW;
navData.holdHeading = AQ_YAW;
// decrease override timer
overrides[2]--;
}
}
#ifdef USE_QUATOS
// determine which frame of reference to control from
if (navData.mode <= NAV_STATUS_ALTHOLD)
// craft frame - manual
{
eulerToQuatYPR(quatDesired, controlData.yaw, controlData.userPitchTarget, controlData.userRollTarget);
} else
// world frame - autonomous
{
eulerToQuatRPY(quatDesired, controlData.navRollTarget, controlData.navPitchTarget, controlData.yaw);
}
// reset controller on startup
if (motorsData.throttle == 0) {
quatDesired[0] = UKF_Q1;
quatDesired[1] = UKF_Q2;
quatDesired[2] = UKF_Q3;
quatDesired[3] = UKF_Q4;
quatosReset(quatDesired);
}
ratesActual[0] = IMU_DRATEX + UKF_GYO_BIAS_X;
ratesActual[1] = IMU_DRATEY + UKF_GYO_BIAS_Y;
ratesActual[2] = IMU_DRATEZ + UKF_GYO_BIAS_Z;
quatos(&UKF_Q1, quatDesired, ratesActual, ratesDesired, overrides);
quatosPowerDistribution(throttle);
motorsSendThrust();
motorsData.throttle = throttle;
#else
// smooth
controlData.userPitchTarget = utilFilter3(controlData.userPitchFilter, controlData.userPitchTarget);
controlData.userRollTarget = utilFilter3(controlData.userRollFilter, controlData.userRollTarget);
// smooth
controlData.navPitchTarget = utilFilter3(controlData.navPitchFilter, controlData.navPitchTarget);
controlData.navRollTarget = utilFilter3(controlData.navRollFilter, controlData.navRollTarget);
// rotate nav's NE frame of reference to our craft's local frame of reference
pitch = controlData.navPitchTarget * navUkfData.yawCos - controlData.navRollTarget * navUkfData.yawSin;
roll = controlData.navRollTarget * navUkfData.yawCos + controlData.navPitchTarget * navUkfData.yawSin;
// combine nav & user requests (both are already smoothed)
controlData.pitch = pitch + controlData.userPitchTarget;
controlData.roll = roll + controlData.userRollTarget;
if (p[CTRL_PID_TYPE] == 0) {
// pitch angle
pitchCommand = pidUpdate(controlData.pitchAnglePID, controlData.pitch, AQ_PITCH);
// rate
pitchCommand += pidUpdate(controlData.pitchRatePID, 0.0f, IMU_DRATEY);
// roll angle
rollCommand = pidUpdate(controlData.rollAnglePID, controlData.roll, AQ_ROLL);
// rate
rollCommand += pidUpdate(controlData.rollRatePID, 0.0f, IMU_DRATEX);
} else if (p[CTRL_PID_TYPE] == 1) {
// pitch rate from angle
pitchCommand = pidUpdate(controlData.pitchRatePID, pidUpdate(controlData.pitchAnglePID, controlData.pitch, AQ_PITCH), IMU_DRATEY);
// roll rate from angle
rollCommand = pidUpdate(controlData.rollRatePID, pidUpdate(controlData.rollAnglePID, controlData.roll, AQ_ROLL), IMU_DRATEX);
}
else if (p[CTRL_PID_TYPE] == 2) {
// pitch angle
pitchCommand = pidUpdate(controlData.pitchAnglePID, controlData.pitch, AQ_PITCH);
// rate
pitchCommand += pidUpdate(controlData.pitchRatePID, 0.0f, IMU_DRATEY);
int axis = 0; // ROLL
float PID_P = 3.7;
float PID_I = 0.031;
float PID_D = 23.0;
float error, errorAngle, AngleRateTmp, RateError, delta, deltaSum;
float PTerm, ITerm, PTermACC = 0, ITermACC = 0, PTermGYRO = 0, ITermGYRO = 0, DTerm;
static int16_t lastGyro[3] = { 0, 0, 0 };
static float delta1[3], delta2[3];
static float errorGyroI[3] = { 0, 0, 0 }, errorAngleI[2] = { 0, 0 };
static float lastError[3] = { 0, 0, 0 }, lastDTerm[3] = { 0, 0, 0 }; // pt1 element http://www.multiwii.com/forum/viewtopic.php?f=23&t=2624;
static int16_t axisPID[3];
static float rollPitchRate = 0.0;
static float newpidimax = 0.0;
float dT;
uint8_t ANGLE_MODE = 0;
uint8_t HORIZON_MODE = 0;
uint16_t cycleTime = IMU_LASTUPD -
controlData.lastUpdate; // this is the number in micro second to achieve a full loop, it can differ a little and is taken into account in the PID loop
if ((ANGLE_MODE || HORIZON_MODE)) { // MODE relying on ACC
errorAngle = constrainFloat(2.0f * (float)controlData.roll, -500.0f, +500.0f) - AQ_ROLL;
}
if (!ANGLE_MODE) { //control is GYRO based (ACRO and HORIZON - direct sticks control is applied to rate PID
AngleRateTmp = (float)(rollPitchRate + 27) * (float)controlData.roll *
0.0625f; // AngleRateTmp = ((int32_t) (cfg.rollPitchRate + 27) * rcCommand[axis]) >> 4;
if (HORIZON_MODE) {
AngleRateTmp += PID_I * errorAngle *
0.0390625f; //increased by x10 //0.00390625f AngleRateTmp += (errorAngle * (float)cfg.I8[PIDLEVEL]) >> 8;
}
} else { // it's the ANGLE mode - control is angle based, so control loop is needed
AngleRateTmp = PID_P * errorAngle * 0.0223214286f; // AngleRateTmp = (errorAngle * (float)cfg.P8[PIDLEVEL]) >> 4; * LevelPprescale;
}
RateError = AngleRateTmp - IMU_DRATEX;
PTerm = PID_P * RateError * 0.0078125f;
errorGyroI[axis] += PID_I * RateError * (float)cycleTime / 2048.0f;
errorGyroI[axis] = constrainFloat(errorGyroI[axis], -newpidimax, newpidimax);
ITerm = errorGyroI[axis] / 8192.0f;
delta = RateError - lastError[axis];
lastError[axis] = RateError;
delta = delta * 16383.75f / (float)cycleTime;
deltaSum = delta1[axis] + delta2[axis] + delta;
delta2[axis] = delta1[axis];
delta1[axis] = delta;
DTerm = PID_D * deltaSum * 0.00390625f;
axisPID[axis] = PTerm + ITerm + DTerm;
rollCommand = AngleRateTmp;
}
else {
pitchCommand = 0.0f;
rollCommand = 0.0f;
ruddCommand = 0.0f;
}
// yaw rate override?
if (overrides[2] > 0)
// manual yaw rate
{
ruddCommand = pidUpdate(controlData.yawRatePID, ratesDesired[2], IMU_DRATEZ);
} else
// seek a 0 deg difference between hold heading and actual yaw
{
ruddCommand = pidUpdate(controlData.yawRatePID, pidUpdate(controlData.yawAnglePID, 0.0f, compassDifference(controlData.yaw, AQ_YAW)),
IMU_DRATEZ);
}
rollCommand = constrainFloat(rollCommand, -p[CTRL_MAX], p[CTRL_MAX]);
pitchCommand = constrainFloat(pitchCommand, -p[CTRL_MAX], p[CTRL_MAX]);
ruddCommand = constrainFloat(ruddCommand, -p[CTRL_MAX], p[CTRL_MAX]);
motorsCommands(throttle, pitchCommand, rollCommand, ruddCommand);
#endif
}
// no throttle input
else {
supervisorThrottleUp(0);
motorsOff();
}
}
// not armed
else {
motorsOff();
}
controlData.lastUpdate = IMU_LASTUPD;
controlData.loops++;
}
}
/*****************************************************
* Timer 5 is used to provide the main system clock
* It generates an interrupt every millisecond
*
* This ISR is where most of the actual work gets done.
* with the sensors running and the straights profiler
* active, this interrupt takes about 220us of which
* 120us is processing the sensors
*****************************************************/
void _ISR SYSTIM_INTERRUPT(void)
{
int pidOut;
unsigned char rxBytes;
/* reset the interrupt flag */
SYSTIM_IF = 0;
//LED_ON;
tickCount++;
millisecondCount--;
commCount++;
if(!GDO0)
{
rxBytes = CC2500_receive_packet();
if(rxBytes>PACKET_LEN)
{
CC2500_idle_mode();
CC2500_clear_rx_fifo();
CC2500_clear_tx_fifo();
CC2500_receive_mode();
commEnabled = FALSE;
}
else
commEnabled = TRUE;
if(newPacket)
{
deassamble_packet();
getFellowCoveredSqrs();
my.obzClear = OBZ_clear();
if(inOBZ(fellow.location))
{
LED_ON;
}
else
{
LED_OFF;
}
dataUpdated = TRUE;
newPacket = FALSE;
noCommCount = 0;
}
else
noCommCount++;
}
if((commCount>=6)&&commEnabled)
{
assamble_packet();
if(!GDO0)
{
CC2500_transmit_packet();
commCount = 0;
}
}
else if(commCount==4)
{
CC2500_idle_mode();
__delay_us(1);
CC2500_clear_rx_fifo();
__delay_us(1);
CC2500_clear_tx_fifo();
__delay_us(1);
CC2500_receive_mode();
}
if(!(tickCount&1))
readCubeSensors();
if(tickCount>300000L)
{
motorsOff();
sensorsOff();
stopSystemTimer();
LED_ON;
}
doButtons();
readLineSensors();
readCounters();
doProfiler();
pidOut = doPID( &left_PID_param);
if( pidOut < -MOTORS_MAX_DC )
pidOut = -MOTORS_MAX_DC;
else if( pidOut > MOTORS_MAX_DC )
pidOut = MOTORS_MAX_DC;
motorsLeftSetDutyCycle(pidOut);
pidOut = doPID( &right_PID_param);
if( pidOut < -MOTORS_MAX_DC )
pidOut = -MOTORS_MAX_DC;
else if( pidOut > MOTORS_MAX_DC )
pidOut = MOTORS_MAX_DC;
motorsRightSetDutyCycle(pidOut);
//LED_OFF;
}
int main(void) {
SYSTEMConfigPerformance(40000000); //System clock is 40MHz
//Because interrupts are good to have
enableInterrupts();
//Because ADC is good to have
initADC();
//Because a timer is good to have for PWM
initTimer2();
//Because PWM is good to have
initPWM();
// initMotorDirPins();
//Because the delayUs function is typically pretty good to have
initTimer4();
//Because UART is good to have for interfacing with our speech chip
initUART(40000000);
//Because a talking robot is the absolute best thing to have
//initSpeakJet();
initSevenSegment();
initSW();
int HelloBytes[] = {0, 0, 0, 0b01110110, 0b01111001, 0b00111000, 0b00111000, 0b00111111, 0, 0, 0};
int helloCounter = 2;
int myCounter = 0;
int timesInFwd = 0;
//sendSpeakjet(initByteArray);
while(1) {
if(millis >= 5000) //if it's been some seconds
{
millis = 0; //reset the timer
//tell speakjet to speak state here
sendSpeakjet(AllYour);
}
if(ScrollMillis >= 600)
{
ScrollMillis = 0;
helloCounter = ((helloCounter + 1) % 8);
sevenSeg_write(0, HelloBytes[WRAPPOS((helloCounter-3), 8)]);
sevenSeg_write(1, HelloBytes[WRAPPOS(helloCounter-2,8)]);
sevenSeg_write(2, HelloBytes[(helloCounter-1) % 8]);
sevenSeg_write(2, HelloBytes[WRAPPOS((helloCounter-1), 8)]);
sevenSeg_write(3, HelloBytes[(helloCounter)]);
}
switch(myState) {
case idle:
motorsOff();
break;
case init:
//Wait for a button press to start us up
motorsOff();
sendSpeakjet(BiteMy);
delayUs(3000000);
myState = backwards;
break;
case backwards:
timesInFwd = 0;
LeftMotor_Write(-800);
RightMotor_Write(-800);
if(leftPairReading < THRESHOLD && rightPairReading < THRESHOLD)
myState = pivot_right;
else if (rightPairReading < THRESHOLD)
myState = turnL_back;
else if (leftPairReading < THRESHOLD)
myState = turnR_back;
else
myState = backwards;
break;
case turnR_back:
turnRight_back(73); //makes the LEFT motor turn at near full power. Yes, it IS the left motor and not the right one.
turnLeft_back(300);
sampleIRPairs(&leftPairReading, &rightPairReading, 10);
// myState = (rightPairReading >= THRESHOLD) ? forward : turnR;
if (leftPairReading < THRESHOLD && rightPairReading < THRESHOLD) //both dark
myState = pivot_right;
else if (rightPairReading < THRESHOLD)
myState = turnR_back;
else if (leftPairReading < THRESHOLD)
myState = turnL_back;
else
myState = backwards;
break;
case turnL_back:
turnRight_back(300); //makes the LEFT motor turn at near full power. Yes, it IS the left motor and not the right one.
turnLeft_back(73);
sampleIRPairs(&leftPairReading, &rightPairReading, 10);
// myState = (rightPairReading >= THRESHOLD) ? forward : turnR;
if (leftPairReading < THRESHOLD && rightPairReading < THRESHOLD) //both dark
myState = pivot_right;
else if (rightPairReading < THRESHOLD)
myState = turnR_back;
else if (leftPairReading < THRESHOLD)
myState = turnL_back;
else
myState = backwards;
break;
case pivot_right:
// if(leftPairReading >= THRESHOLD) //left is light
// myState = forward;
// break;
RightMotor_Write(-650);
LeftMotor_Write(700);
if(rightPairReading > THRESHOLD)
myState = forward;
break;
case turnR:
// turnRight(512);
turnRight(73); //makes the LEFT motor turn at near full power. Yes, it IS the left motor and not the right one.
turnLeft(800);
sampleIRPairs(&leftPairReading, &rightPairReading, 10);
// myState = (rightPairReading >= THRESHOLD) ? forward : turnR;
if (leftPairReading < THRESHOLD && rightPairReading < THRESHOLD) //both dark
myState = turnR;
else if (rightPairReading < THRESHOLD)
myState = turnR;
else if (leftPairReading < THRESHOLD)
myState = turnL;
else
myState = forward;
break;
case turnL:
// turnLeft(512);
turnLeft(73); //makes RIGHT motor run at near full power. Yes, it IS the right motor, not the left one.
turnRight(800);
sampleIRPairs(&leftPairReading, &rightPairReading, 10);
// myState = (leftPairReading >= THRESHOLD) ? forward : turnL;
if (leftPairReading < THRESHOLD && rightPairReading < THRESHOLD)
myState = turnR;
else if (rightPairReading < THRESHOLD)
myState = turnR;
else if (leftPairReading < THRESHOLD)
myState = turnL;
else
myState = forward;
break;
case forward:
if(timesInFwd >= 25000)
{
sendSpeakjet(finishedPhrase);
myState = backwards;
// myState = idle;
break;
}
timesInFwd++;
goForward();
sampleIRPairs(&leftPairReading, &rightPairReading, 100);
if (rightPairReading < THRESHOLD) {
myState = turnR;
timesInFwd = 0;
}
else if (leftPairReading < THRESHOLD) {
myState = turnL;
timesInFwd = 0;
}
break;
}
}
return (EXIT_SUCCESS);
}
