void adjust_duty_cycle(double desired_torque) {
double current_torque = get_current_torque();
double torque_delta = desired_torque - current_torque; // Between -2.0 and 2.0
double normalized_torque_delta = torque_delta / 2.0; // Between -1.0 and 1.0
double duty_cycle_adjustment = (DUTY_CYCLE_MAX_ADJUSTMENT * normalized_torque_delta);
set_duty_cycle(haptics_duty_cycle + duty_cycle_adjustment);
}
void scissorMotorControl(double direction, double speed) {
set_up_pwm();
double dirInRads = (direction * M_PI) / 180.0;
double dutyCycle1 = (speed * cos(((150.0 * M_PI) / 180.0) - dirInRads));
//uint16_t direction1 = (dutyCycle1 > 0.0) ? 1 : 0;
dutyCycle1 = (dutyCycle1 < 0.0) ? (dutyCycle1 * -1.0) : dutyCycle1;
set_duty_cycle(dutyCycle1);
}
void set_up_pwm() {
pwm_gpt2_pwm1_init();
pwm_gpt2_pwm1_config_gpio(GPIO_B_NUM, 4);
pwm_gpt2_pwm1_set_timing(PWM_PERIOD_MS, 0.0);
pwm_gpt2_pwm1_start();
pwm_gpt2_pwm2_init();
pwm_gpt2_pwm2_config_gpio(GPIO_B_NUM, 4);
pwm_gpt2_pwm2_set_timing(PWM_PERIOD_MS, 0.0);
pwm_gpt2_pwm2_start();
set_duty_cycle(0.0);
}
static ssize_t set_enable(struct device *dev, struct device_attribute *da,
const char *buf, size_t count)
{
struct as7712_32x_fan_data *data = as7712_32x_fan_update_device(dev);
int error, value;
error = kstrtoint(buf, 10, &value);
if (error)
return error;
if (value < 0 || value > 1)
return -EINVAL;
data->enable = value;
if (value == 0)
{
return set_duty_cycle(dev, da, buf, FAN_MAX_DUTY_CYCLE);
}
return count;
}
__task void adc_test(void)
{
float sol_v, sol_i, batt_v, batt_i, temp;
init_pwm(40000);
init_adc();
set_duty_cycle(82);
while (1)
{
sol_v = get_adc_voltage(ADC_SOL_V);
sol_i = get_adc_voltage(ADC_SOL_I);
batt_v = get_adc_voltage(ADC_BATT_V);
batt_i = get_adc_voltage(ADC_BATT_I);
temp = get_adc_voltage(ADC_TEMP);
TRACE_INFO("6,%f,%f,%f,%f,%f\n", sol_v, sol_i, batt_v, batt_i, temp );
os_dly_wait(100);
}
}
__task void interrupted_charging (void)
{
float sol_voltage = 0.0f, sol_current = 0.0f, sol_power = 0.0f;
int pulse = 0;
int counter = 0;
//TODO: initialise hardware
init_pwm(40000);
init_adc();
set_mppt();
while (1)
{
batt_voltage = get_adc_voltage(ADC_BATT_V);
batt_current = get_adc_voltage(ADC_BATT_I);
sol_voltage = get_adc_voltage(ADC_SOL_V);
sol_current = get_adc_voltage(ADC_SOL_I);
sol_power = sol_voltage * sol_current;
temp = get_adc_voltage(ADC_TEMP);
set_temperature_compensation( temp, &v_high, &pulse_duty );
TRACE_INFO("1,%.0f,%i,%.2f,%.2f,%.2f,%.3f,%.1f,%.2F\n",
((double)get_time_t()), cc_state, batt_voltage, batt_current, sol_voltage, sol_current, duty_cycle, temp);
//Check for LVDC voltage
calc_lvdc(batt_current);
if ( batt_voltage < v_lvdc )
{
//Turn off outputs and screen.
//send os event to turn off
os_evt_set( UI_LVDC, ui_t );
}
switch (cc_state)
{
case BULK_CHARGING:
//Start charging battery with 0.1C current or as high as possible if 0.1C cannot be met
perturb_and_observe_cc_itter(BATTERY_AHR*0.1f);
if (++counter > 10)
set_current_compensation(BATTERY_AHR*0.1f, batt_current, &v_high);
if (sol_power < P_NIGHT_MODE)
{
if (set_mppt() < (P_NIGHT_MODE*1.2))
{
cc_state = NIGHT_MODE;
counter = 0;
TRACE_DEBUG("Starting Night Mode State\n");
break;
}
TRACE_DEBUG("Rescaned Power and Night mode not entered \n");
}
if (batt_voltage > v_high)
{
cc_state = VOLTAGE_SETTLE;
counter = 0;
TRACE_DEBUG("Starting Voltage Settle Charging State at V=%f \n", batt_voltage);
break;
}
os_dly_wait( P_AND_O_PERIOD );
break;
case VOLTAGE_SETTLE:
//Diable the MPPT Charging Circuit
GPIO_ResetBits(GPIOB, GPIO_Pin_0);
if (batt_voltage < v_low)
{
GPIO_SetBits(GPIOB, GPIO_Pin_0);
cc_state = PULSED_CURRENT;
TRACE_DEBUG("Starting Pulsed Current Charging State at V=%f \n", batt_voltage);
break;
}
//5s delay
os_dly_wait(50);
break;
case PULSED_CURRENT:
counter++;
if (pulse)
{
//If greater than high period seconds, pulse is low. i.e sets up x% duty cycle
if ( counter > (300 * pulse_duty) )
pulse = 0;
//Enable MPPT hardware
GPIO_SetBits(GPIOB, GPIO_Pin_0);
//Run p&o itteration
perturb_and_observe_cc_itter(BATTERY_AHR*0.05f);
if ( counter > 10 )
{
//set_current_compensation(BATTERY_AHR*0.05f, batt_current);
if (sol_power < P_NIGHT_MODE)
{
if (set_mppt() < (P_NIGHT_MODE*1.2))
{
cc_state = NIGHT_MODE;
counter = 0;
TRACE_DEBUG("Starting Night Mode State\n");
break;
}
TRACE_DEBUG("Rescaned Power and Night mode not entered \n");
}
}
}
else
{
//If greater than 30s, reset.
if ( counter > 300)
{
counter = 0;
pulse = 1;
}
//Diable the MPPT Charging Circuit
GPIO_ResetBits(GPIOB, GPIO_Pin_0);
}
if (batt_voltage > v_high)
{
counter = 0;
cc_state = FULLY_CHARGED;
TRACE_DEBUG("Starting Fully Charged Charging State at V=%f \n", batt_voltage);
break;
}
//100ms delay
os_dly_wait(10);
break;
case FULLY_CHARGED:
//Regulate so that Battery current = 0
//This means that the battery is no longer charged
//But the panel is used to power any connected load
//Diable the MPPT Charging Circuit
GPIO_ResetBits(GPIOB, GPIO_Pin_0);
//Check when to start recharging again.
if (batt_voltage < v_restart)
{
cc_state = BULK_CHARGING;
TRACE_DEBUG("Restarting the Bulk Charging State \n");
break;
}
//5s delay
os_dly_wait(500);
break;
case NIGHT_MODE:
//Check every 5 minutes
//Counter used so that LVDC is still
//checked every 5 seconds
if (++counter > 2)
{
//Enable MPPT Circuit
GPIO_SetBits(GPIOB, GPIO_Pin_0);
if (set_mppt() > (P_NIGHT_MODE*1.2))
{
cc_state = BULK_CHARGING;
TRACE_DEBUG("Exiting Night Mode\n");
break;
}
//Diable the MPPT Charging Circuit
//GPIO_ResetBits(GPIOB, GPIO_Pin_0);
set_duty_cycle(100);
counter = 0;
}
//5 second wait time
os_dly_wait(500);
break;
default:
cc_state = BULK_CHARGING;
TRACE_ERROR("Charging State machine entered Unknown State. Restarting with Bulk Charging \n");
}
}
}
int main()
{
init();
unsigned int i = 0;
unsigned int uiGapStartEnc = 0;
unsigned int uiSouthEastDis = 0;
short AcX, AcY, AcZ, Tmp, GyX, GyY, GyZ, x, y, z;
char cBuff[32];
bool_t bGapStarted = false;
ParkingStateType_t cs = Ready;
*pwm_enable = 0;
printf("s\n");
#ifdef MOTOR_TEST
volatile unsigned int fr, rr, rl, fl;
set_duty_cycle(pFrontRightDutySet, 50);
set_duty_cycle(pRearRightDutySet, 50);
set_duty_cycle(pRearLeftDutySet, 50);
set_duty_cycle(pFrontLeftDutySet, 50);
*pwm_enable = (ALL_WHEEL_FWD_MASK | ENABLE_ENC_MASK );
delay(10000000);
*pwm_enable = PAUSE_ENC_MASK;
fr = *pFrontRightEncRead;
rr = *pRearRightEncRead;
rl = *pRearLeftEncRead;
fl = *pFrontLeftEncRead;
*pwm_enable = 0;
printf("Front right = %u\n", fr);
printf("rear right = %u\n", rr);
printf("rear left = %u\n", rl);
printf("Front left = %u\n", fl);
printf("Restarting the same number of rotations\n");
delay(100000);
*pFrontRightEncSet = fr;
*pRearRightEncSet = rr;
*pRearLeftEncSet = rl;
*pFrontLeftEncSet = fl;
*pwm_enable = (ALL_WHEEL_FWD_MASK | PLAY_BACK_MASK | ENABLE_ENC_MASK );
while (!(*pwm_enable & WHEEL_READY_MASK));
fr = *pFrontRightEncRead;
rr = *pRearRightEncRead;
rl = *pRearLeftEncRead;
fl = *pFrontLeftEncRead;
*pwm_enable = 0;
printf("Front right = %u\n", fr);
printf("rear right = %u\n", rr);
printf("rear left = %u\n", rl);
printf("Front left = %u\n", fl);
#endif
#ifdef HC_SR04_TEST
while (1)
{
printf("new\n");
while (*pHc_sr04 != 0xff);
//printf("2\n");
*pHc_sr04 = 0xC7;
//printf("3\n");
while (*pHc_sr04 != 0xff);
//delay(10000000);
for (i = 0; i < NUMBER_OF_ULTRA_SOUND_DEVICES; i++)
{
printf("%u = %u\n",i, MeasureDistance(i));
}
printf("\n\n");
delay(10000000);
//printf("4\n");;
}
#endif
#ifdef MPU_TEST
I2CWrite(MPU_SLAVE_ADDRESS, 0x6b, 0x0);
I2CRead(MPU_SLAVE_ADDRESS, 0x75, 1, cBuff);
printf("WhoAmI = %x\n", (unsigned int)cBuff[0]);
while(1)
{
AcX = AcY = AcZ = Tmp = GyX = GyY = GyZ = 0;
I2CRead(MPU_SLAVE_ADDRESS, 0x3B, 14, cBuff);
AcX = (cBuff[0] << 8) | (cBuff[1] & 0xff);
AcY = (cBuff[2] << 8) | (cBuff[3] & 0xff);
AcZ = cBuff[4] << 8 | (cBuff[5] & 0xff);
//AcZ = cBuff[5];
Tmp = (cBuff[6] << 8) | (cBuff[7] & 0xff);
GyX = (cBuff[8] << 8) | (cBuff[9] & 0xff);
GyY = (cBuff[10] << 8) | (cBuff[11] & 0xff);
GyZ = (cBuff[12] << 8) | (cBuff[13] & 0xff);
printf("AcX = %d\n", AcX);
printf("AcY = %d\n", AcY);
printf("AcZ = %d\n", AcZ);
// printf("%u,%u\n", cBuff[4], (cBuff[5] & 0xff));
printf("Tmp = %f\n", (float)Tmp/340 + 36.53);
printf("GyX = %d\n", GyX);
printf("GyY = %d\n", GyY);
printf("GyZ = %d\n", GyZ);
delay(10000000);
}
#endif
#ifdef COMPASS_TEST
I2CWrite(HMC5883L_SLAVE_ADDRESS, 0x02, 00); //set contineous read mode
while(1)
{
I2CRead(HMC5883L_SLAVE_ADDRESS, 0x03, 6, cBuff);
x = (cBuff[0] << 8) | (cBuff[1] & 0xff);
z = (cBuff[2] << 8) | (cBuff[3] & 0xff);
y = (cBuff[4] << 8) | (cBuff[5] & 0xff);
printf("X = %d\n", x);
printf("Y = %d\n", y);
printf("Z = %d\n", z);
delay(10000000);
}
#endif
return 0;
}
