int main(int argc,char **argv) {
int i; //iterator
pid *x,*y,*z; //structs representing 3 pid controllers
imu_t *imu;
fd_set net_set;
double oldmag,magdiff;
struct timeval timeout;
int axes[4]={0,0,0,0};
int x_adjust,y_adjust,z_adjust;
int sock = init_net();
register_int();
x=init_pid(3500,000,000);
y=init_pid(3400,000,1000);
z=init_pid(000,000,000);
timeout.tv_sec=0;
timeout.tv_usec=REFRESH_TIME;
for(i=0;i<4;i++) { //initialize PWM modules
motors[i]=init_pwm(i);
}
imu = init_imu();
update_imu(imu);
oldmag=imu->mpu.fusedEuler[VEC3_Z] * RAD_TO_DEGREE;
while(1) {
////printf("loop\n");
FD_ZERO(&net_set);
FD_SET(sock,&net_set);
if(select(sock+1,&net_set,(fd_set *) 0,(fd_set *) 0, &timeout)==1) {
////printf("reading from the socket\n");
//read from sock
if(!update_axis(sock,axes)) {
break;
}
} else {
////printf("reading from the imu\n");
//update imu
update_imu(imu);
timeout.tv_sec=0;
timeout.tv_usec=REFRESH_TIME;
magdiff=imu->mpu.fusedEuler[VEC3_Z] * RAD_TO_DEGREE-oldmag;
if(magdiff>300) {
magdiff-=360;
}
if(magdiff<-300) {
magdiff+=360;
}
x_adjust = update_pid(x,0/*axes[1]*.00061*/,imu->mpu.fusedEuler[VEC3_X] * RAD_TO_DEGREE);
y_adjust = update_pid(y,0/*axes[2]*.00061*/,imu->mpu.fusedEuler[VEC3_Y] * RAD_TO_DEGREE);
//printf("X axis value: %lf\n",imu->mpu.fusedEuler[VEC3_X]*RAD_TO_DEGREE);
//printf("Y axis value: %lf\n",imu->mpu.fusedEuler[VEC3_Y]*RAD_TO_DEGREE);
z_adjust = update_pid(z,axes[3]*.00061,magdiff);
oldmag=imu->mpu.fusedEuler[VEC3_Z] * RAD_TO_DEGREE;
set_duty(motors[0],900000+axes[0]*16-z_adjust-y_adjust+x_adjust);
set_duty(motors[1],900000+axes[0]*16+z_adjust-y_adjust-x_adjust);
set_duty(motors[2],900000+axes[0]*16-z_adjust+y_adjust-x_adjust);
set_duty(motors[3],900000+axes[0]*16+z_adjust+y_adjust+x_adjust);
}
}
exit_fxn(0);
}
int main()
{
signal(SIGINT, ctrlc);
mapper_device dev = mdev_new("pwm", 9000, 0);
float min0 = 0;
float max1 = 1;
float max1000 = 1000;
mdev_add_input(dev, "/freq", 1, 'f', "Hz", &min0, &max1000,
(mapper_signal_handler*)handler_freq, 0);
mdev_add_input(dev, "/gain", 1, 'f', "Hz", &min0, &max1,
(mapper_signal_handler*)handler_gain, 0);
mdev_add_input(dev, "/duty", 1, 'f', "Hz", &min0, &max1,
(mapper_signal_handler*)handler_duty, 0);
run_synth();
set_duty(0.1);
set_freq(110.0);
set_gain(0.1);
printf("Press Ctrl-C to quit.\n");
while (!done)
mdev_poll(dev, 10);
mdev_free(dev);
set_freq(0);
sleep(1);
stop_synth();
}
void recieve_data_input(void)
{
controller_data();
if(D_direction_R+D_direction_L+D_direction_U+D_direction_D+PSCON_PRE_SANKAKU+PSCON_PRE_MARU+PSCON_PRE_BATSU+PSCON_PRE_SHIKAKU+
PSCON_PRE_L1+PSCON_PRE_R1+PSCON_PRE_L2+PSCON_PRE_R2 > 1000)//暴走防止
{
/* HALF_1=HALF_2=255;
DUTY_L=DEGREE_L=DUTY_R=DEGREE_R=0;
D_direction_R=D_direction_L=D_direction_U=D_direction_D=0;
PSCON_PRE_SANKAKU=PSCON_PRE_MARU=PSCON_PRE_BATSU=PSCON_PRE_SHIKAKU=0;
PSCON_PRE_L1=PSCON_PRE_R1=PSCON_PRE_L2=PSCON_PRE_R2=0;
*/
// set_duty();
// input_PWM_ctrl();
POWER_OUT=0;
pscon_err_flag=1;
}
else if(pscon_err_flag==1)
{
pscon_err_flag=0;
POWER_OUT=1;
}
else set_duty();
// set_duty(); // 値をdutyに変換
}
void handler_duty(mapper_signal msig,
mapper_db_signal props,
mapper_timetag_t *timetag,
void *value)
{
float *pduty = (float*)pduty;
set_duty(*pduty);
}
void exit_fxn(int asdf) {
//printf("quitting\n");
int i=0;
for(i=0;i<4;i++) {
set_duty(motors[i],900000);
}
close(server_socket);
exit(1);
}
void handler_duty(mapper_signal msig,
mapper_db_signal props,
int instance_id,
void *value,
int count,
mapper_timetag_t *timetag)
{
if (value) {
float *pduty = (float*)value;
set_duty(*pduty);
}
}
*/bool wait_for_temperatur(float temp)
{
float te = compute_temperatur();
while (te > (temp - 3))
{
te = compute_temperatur();
set_duty(0);
printf("%f\n", te);
}
do
{
te = compute_temperatur();
set_duty((int) period - 10);
printf("%f\n", te);
} while (!((te > temp) && (te < (temp + 1))));
return true;
}
*/bool check_temperatur(float temp)
{
printf("Checking temp: ");
float te = compute_temperatur();
int8_t temp_too_heigh = false;
while (te > temp)
{
te = compute_temperatur();
set_duty(0);
printf(" Too Hot: %g\n", te);
delay_ms(1000);
temp_too_heigh = true;
}
printf("TEMP OK, %g \n", te);
return temp_too_heigh;
}
void tic_isr_6()
{
uint32_t out_state = 0;
//uart_putstr("Interruption Timer 6\n");
out_state = gpio0->out;
gpio0->out = out_state | 0x08;
timer0->counter6 = 0;
timer0->tcr6 = TIMER_EN | TIMER_AR | TIMER_IRQEN;
timer0->compare7 = set_duty(pwm_d[3]);
timer0->counter7 = 0;
timer0->tcr7 = TIMER_EN | TIMER_AR | TIMER_IRQEN;
}
void tic_isr_4()
{
uint32_t out_state = 0;
//uart_putstr("Interruption Timer 4\n");
out_state = gpio0->out;
gpio0->out = out_state | 0x04;
timer0->counter4 = 0;
timer0->tcr4 = TIMER_EN | TIMER_AR | TIMER_IRQEN;
timer0->compare5 = set_duty(pwm_d[2]);
timer0->counter5 = 0;
timer0->tcr5 = TIMER_EN | TIMER_AR | TIMER_IRQEN;
}
void tic_isr_2()
{
uint32_t out_state = 0;
//uart_putstr("Interruption Timer 2\n");
out_state = gpio0->out;
gpio0->out = out_state | 0x02;
timer0->counter2 = 0;
timer0->tcr2 = TIMER_EN | TIMER_AR | TIMER_IRQEN;
timer0->compare3 = set_duty(pwm_d[1]);
timer0->counter3 = 0;
timer0->tcr3 = TIMER_EN | TIMER_AR | TIMER_IRQEN;
}
void tic_isr_0()
{
uint32_t out_state = 0;
//uart_putstr("Interruption Timer 0\n");
out_state = gpio0->out;
gpio0->out = out_state | 0x01;
timer0->counter0 = 0;
timer0->tcr0 = TIMER_EN | TIMER_AR | TIMER_IRQEN;
timer0->compare1 = set_duty(pwm_d[0]);
timer0->counter1 = 0;
timer0->tcr1 = TIMER_EN | TIMER_AR | TIMER_IRQEN;
}
// Test the mosfet
void mosfet_start_up_test(void)
{
on_tim1_cc4 = gather_adc_values;
TIM_ITConfig(TIM1, TIM_IT_CC4, ENABLE);
mosfet_test_step = 1;
// apply the step
mosfet_test_update_phase();
// trggier a COM event
TIM_GenerateEvent(TIM1, TIM_EventSource_COM);
set_duty(DUTY(10), DUTY(50)); // PWM 10% duty, CC4 20%
set_phase_update_freq(G5_Tone*2, mosfet_test_update_phase);
initial_adc_dma_for_mosfet_test();
start_adc(CH_A, ADC_SampleTime_1_5Cycles);
while(1);
}
void handler_duty(mapper_signal sig, float *pduty)
{
set_duty(*pduty);
}
void tic_init() //Inicialización de el timer
{
// Set high M1,M2,M3 and M4
gpio0->out=0x0F;
// Setup timer0.0
timer0->compare0 = set_period();
timer0->counter0 = 0;
timer0->tcr0 = TIMER_EN | TIMER_AR | TIMER_IRQEN;
// Setup timer0.1
timer0->compare1 = set_duty(pwm_d[0]);
timer0->counter1 = 0;
timer0->tcr1 = TIMER_EN | TIMER_AR | TIMER_IRQEN;
// Setup timer0.2
timer0->compare2 = set_period();
timer0->counter2 = 0;
timer0->tcr2 = TIMER_EN | TIMER_AR | TIMER_IRQEN;
// Setup timer0.3
timer0->compare3 = set_duty(pwm_d[1]);
timer0->counter3 = 0;
timer0->tcr3 = TIMER_EN | TIMER_AR | TIMER_IRQEN;
// Setup timer0.4
timer0->compare4 = set_period();
timer0->counter4 = 0;
timer0->tcr4 = TIMER_EN | TIMER_AR | TIMER_IRQEN;
// Setup timer0.5
timer0->compare5 = set_duty(pwm_d[2]);
timer0->counter5 = 0;
timer0->tcr5 = TIMER_EN | TIMER_AR | TIMER_IRQEN;
// Setup timer0.6
timer0->compare6 = set_period();
timer0->counter6 = 0;
timer0->tcr6 = TIMER_EN | TIMER_AR | TIMER_IRQEN;
// Setup timer0.7
timer0->compare7 = set_duty(pwm_d[3]);
timer0->counter7 = 0;
timer0->tcr7 = TIMER_EN | TIMER_AR | TIMER_IRQEN;
isr_register(3, &tic_isr_0);
isr_register(4, &tic_isr_1);
isr_register(5, &tic_isr_2);
isr_register(6, &tic_isr_3);
isr_register(7, &tic_isr_4);
isr_register(8, &tic_isr_5);
isr_register(9, &tic_isr_6);
isr_register(10, &tic_isr_7);
//Button function register
isr_register(15, &tic_isr_15);
isr_register(16, &tic_isr_16);
isr_register(17, &tic_isr_17);
isr_register(18, &tic_isr_18);
isr_register(19, &tic_isr_19);
}
int main(void) {
// UDP Creation
struct sockaddr_in si_me, si_other;
int s, slen = sizeof(si_other) , recv_len;
char buf[BUFLEN];
char motor1_2, motor1_1,motor2_2, motor2_1,motor3_2, motor3_1,motor4_2, motor4_1,motor5_2, motor5_1,motor6_2, motor6_1;
char motor1[3], motor2[3], motor3[3], motor4[3], motor5[3], motor6[3];
//create a UDP socket
if ((s=socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP)) == -1)
{
die("socket");
}
// zero out the structure
memset((char *) &si_me, 0, sizeof(si_me));
si_me.sin_family = AF_INET;
si_me.sin_port = htons(PORT);
si_me.sin_addr.s_addr = htonl(INADDR_ANY);
//bind socket to port
if( bind(s , (struct sockaddr*)&si_me, sizeof(si_me) ) == -1)
{
die("bind");
}
//Set up pwms
char freq[5];
int i, motor_num;
set_pwms();
//Ask the user what the frequency should be
printf("What should the frequency be? ");
fflush(stdout);
scanf("%s", freq);
//Set all frequencies to the user input
for(i='1';i<'7';i++){
set_freq(i,freq);
}
//Activate the run file in each pwm folder
for(i='1';i<'7';i++){
set_run(i,"1");
}
//Set the duty percent to 50 for all pwms
for(i='1';i<'7';i++){
set_duty(i,"50");
}
while(1){
//try to receive some data
if ((recv_len = recvfrom(s, buf, BUFLEN, 0, (struct sockaddr *) &si_other, &slen)) == -1)
{
die("recvfrom()");
}
// Figure out the number of motors from first three bytes of the array received from UDP packet
motor_num = buf[3];
int motors[motor_num];
// Create an array that has the duty percent for each motor in the elements
for(i=0; i<motor_num; i++){
motors[i] = buf[i+4];
if(motors[i] > 100) motors[i] = motors[i] - 256; //Conversion for numbers that should be negative
}
fflush(stdout);
//Change the percentages from -100-100 to 0-100 because that's the percentages the BeagleBone can do
for(i=0; i<6; i++){
motors[i] = (motors[i] + 100)/2;
}
// The next bunch of if else statements convert the integer duty percent number into strings that the
// Beaglebone can understand. Each statement will go through the first and second digit of the integer
// and then change them to ASCII code and put them into an array.
if(motors[0] == 100){
motor1[0] = '1';
motor1[1] = '0';
motor1[2] = '0';
motor1[3] = '\0';
}
else {
motor1_2 = motors[0]%10 + 0x30;
motor1_1 = (motors[0]/10)%10 + 0x30;
motor1[0] = '0';
motor1[1] = motor1_1;
motor1[2] = motor1_2;
motor1[3] = '\0';
}
if(motors[1] == 100){
motor2[0] = '1';
motor2[1] = '0';
motor2[2] = '0';
motor2[3] = '\0';
}
else {
motor2_2 = motors[1]%10 + 0x30;
motor2_1 = (motors[1]/10)%10 + 0x30;
motor2[0] = '0';
motor2[1] = motor2_1;
motor2[2] = motor2_2;
motor2[3] = '\0';
}
if(motors[2] == 100){
motor3[0] = '1';
motor3[1] = '0';
motor3[2] = '0';
motor3[3] = '\0';
}
else {
motor3_2 = motors[2]%10 + 0x30;
motor3_1 = (motors[2]/10)%10 + 0x30;
motor3[0] = '0';
motor3[1] = motor3_1;
motor3[2] = motor3_2;
motor3[3] = '\0';
}
if(motors[3] == 100){
motor4[0] = '1';
motor4[1] = '0';
motor4[2] = '0';
motor4[3] = '\0';
}
else {
motor4_2 = motors[3]%10 + 0x30;
motor4_1 = (motors[3]/10)%10 + 0x30;
motor4[0] = '0';
motor4[1] = motor4_1;
motor4[2] = motor4_2;
motor4[3] = '\0';
}
if(motors[4] == 100){
motor5[0] = '1';
motor5[1] = '0';
motor5[2] = '0';
motor5[3] = '\0';
}
else {
motor5_2 = motors[4]%10 + 0x30;
motor5_1 = (motors[4]/10)%10 + 0x30;
motor5[0] = '0';
motor5[1] = motor5_1;
motor5[2] = motor5_2;
motor5[3] = '\0';
}
if(motors[5] == 100){
motor6[0] = '1';
motor6[1] = '0';
motor6[2] = '0';
motor6[3] = '\0';
}
else {
motor6_2 = motors[5]%10 + 0x30;
motor6_1 = (motors[5]/10)%10 + 0x30;
motor6[0] = '0';
motor6[1] = motor6_1;
motor6[2] = motor6_2;
motor6[3] = '\0';
}
// Print all the duty percents that were calculated
printf("First Motor: %s\n", motor1);
printf("Second Motor: %s\n", motor2);
printf("Third Motor: %s\n", motor3);
printf("Fourth Motor: %s\n", motor4);
printf("Fifth Motor: %s\n", motor5);
printf("Sixth Motor: %s\n\n", motor6);
// Set the duty percent of each motor
set_duty('1', motor1);
set_duty('2', motor2);
set_duty('3', motor3);
set_duty('4', motor4);
set_duty('5', motor5);
set_duty('6', motor6);
}
close(s);
return 0;
}
