void set_pisak(int i){
if(i){
set_PWM(5);
}
else{
set_PWM(15);
}
}
void setup_movement(void)
{
serial_configure(9600);
set_PWM(LEF1, 0);
set_PWM(LEF0, 0);
set_PWM(RIG0, 0);
set_PWM(RIG1, 0);
start_encoder();
}
void SMSpeaker_tick(){
unsigned char bOnInput = ONBUTTON;
switch(StateSpeaker){
case SPEAKER_INIT:
break;
case SPEAKER_OFF:
PORTD = 0x01;
PWM_off();
break;
case SPEAKER_ON:
PORTD = 0x02;
PWM_on();
set_PWM(pitchScale[currPitch]);
break;
case SPEAKER_OFF_WAIT:
PORTD = 0x04;
break;
case SPEAKER_ON_WAIT:
PORTD = 0x08;
set_PWM(pitchScale[currPitch]);
break;
default:
break;
}
switch(StateSpeaker){
case SPEAKER_INIT:
StateSpeaker = SPEAKER_OFF_WAIT;
break;
case SPEAKER_OFF:
if(bOnInput == 0){
StateSpeaker = SPEAKER_OFF_WAIT;
}
break;
case SPEAKER_ON:
if(bOnInput == 0){
StateSpeaker = SPEAKER_ON_WAIT;
}
break;
case SPEAKER_OFF_WAIT:
if(bOnInput != 0){
StateSpeaker = SPEAKER_ON;
}
break;
case SPEAKER_ON_WAIT:
if(bOnInput != 0){
StateSpeaker = SPEAKER_OFF;
}
default:
break;
}
return 0;
}
void setup_movement(void)
{
DDRC |= (1 << DDC4);
DDRC |= (1 << DDC0);
PORTC |= (1 << PC4);
PORTC |= (1 << PC0);
//a4 and a0 pc4 and pc0
set_PWM(LEF1, 0);
set_PWM(LEF0, 0);
set_PWM(RIG0, 0);
set_PWM(RIG1, 0);
start_encoder();
}
void PWM_on() {
TCCR3A = (1 << COM3A0);
// COM3A0: Toggle PB6 on compare match between counter and OCR3A
TCCR3B = (1 << WGM32) | (1 << CS31) | (1 << CS30);
// WGM32: When counter (TCNT3) matches OCR3A, reset counter
// CS31 & CS30: Set a prescaler of 64
set_PWM(0);
}
void reset_PWM(void)
{
Current_state.pwm.x = 0;
Current_state.pwm.y = 0;
Current_state.pwm.z = 0;
Current_state.pwm.x_dir = 0;
Current_state.pwm.y_dir = 0;
Current_state.pwm.z_dir = 0;
set_PWM();
}
void back_R(void)
{
set_PWM(RIG0, 0);
set_PWM(RIG1, pow_right);
}
void back_L(void)
{
set_PWM(LEF0, 0);
set_PWM(LEF1, pow_left);
}
void ahead_L(void)
{
set_PWM(LEF0, pow_left);
set_PWM(LEF1, 0);
}
void ahead_R(void)
{
set_PWM(RIG0, pow_right);
set_PWM(RIG1, 0);
}
int main(void)
{
/// Define Variables to store Magnetic field
// char array[40];
// char array1[40];
// char array2[40];
/// Blink LED to show that program is successfully running
DDRA = 0xF0;
PORTA = 0xF0;
_delay_ms(500);
PORTA = 0x00;
_delay_ms(500);
PORTA = 0xF0;
_delay_ms(500);
PORTA = 0x00;
_delay_ms(500);
/// Inittialise UART0 for Transmission to terminal
init_UART0();
/// Transmit "Hello" String
transmit_UART0('H');
transmit_UART0('e');
transmit_UART0('l');
transmit_UART0('l');
transmit_UART0('o');
// sprintf(array1,"\tThis is PRATHAM's OBC-Master code...");
// sprintf(array2,"\rCurrent MagnetoMeter and Torquer state is =\t");
// transmit_string_UART0(array1);
// transmit_string_UART0(array2);
/// Configure the Magnetometer
init_UART_MM();
/// Configure the Torquer
configure_torquer();
/// Define 3 strings for storing Magnetometer field values
char sx[20];
char sy[20];
char sz[20];
Time = 0;
// timer_reset_teo_sec();
while (1)
{
/// check for 2 seconds
// if(tot_overflow >= 30)
// {
// Time += 2;
// reset_PWM();
// send_MM_cmd("*00P\r");
// poll_MM();
// TCNT1 = 0;
// tot_overflow = 0;
// }
/// Read Magmeter every time Simulink sends something whether 0 or non-zero
// read_MM();
Bx=(int16_t)receive_MM();
Bx=(Bx<<8);
Bx &= 0xFF00;
Bx|=(int16_t)receive_MM();
By=(int16_t)receive_MM();
By=(By<<8); By &= 0xFF00;
By|=(int16_t)receive_MM();
Bz=(int16_t)receive_MM();
Bz=(Bz<<8);
Bz &= 0xFF00;
Bz|=(int16_t)receive_MM();
/// Receive carriage return and ignore
receive_MM();
/// when atleast one of the values is non-zero, execute the control Law
if (Bx !=0x00 || By != 0x00 || Bz != 0x00)
{
/// Apply control Law
control();
set_PWM();
/// Transmit Magnetic field Data to terminal
sprintf(sx,"%d",Bx);
transmit_UART0('X');
transmit_string_UART0(sx);
transmit_UART0(' ');
sprintf(sy,"%d",By);
transmit_UART0('Y');
transmit_string_UART0(sy);
transmit_UART0(' ');
sprintf(sz,"%d",Bz);
transmit_UART0('Z');
transmit_string_UART0(sz);
transmit_UART0(' ');
transmit_UART0('\r');
////////////////////////////////////////////
sprintf(sx,"%d",vg[0]);
transmit_UART0('A');
transmit_string_UART0(sx);
transmit_UART0(' ');
sprintf(sx,"%d",vg[1]);
transmit_UART0('B');
transmit_string_UART0(sx);
transmit_UART0(' ');
sprintf(sx,"%d",vg[2]);
transmit_UART0('C');
transmit_string_UART0(sx);
transmit_UART0(' ');
transmit_UART0('\r');
////////////////////////////
// sprintf(sx,"%d",norm_vect);
// transmit_UART0('D');
// transmit_string_UART0(sx);
// transmit_UART0(' ');
// transmit_UART0('\r');
///////////////////////////
// sprintf(sx,"%d",Bdot[0]);
// transmit_UART0('P');
// transmit_string_UART0(sx);
// transmit_UART0(' ');
// transmit_UART0('\r');
//
// sprintf(sy,"%d",Bdot[1]);
// transmit_UART0('Q');
// transmit_string_UART0(sy);
// transmit_UART0(' ');
// transmit_UART0('\r');
//
// sprintf(sz,"%d",Bdot[2]);
// transmit_UART0('R');
// transmit_string_UART0(sz);
// transmit_UART0(' ');
// transmit_UART0('\r');
//
// //////////////////////////
//
// sprintf(sx,"%d",temp[0]);
// transmit_UART0('L');
// transmit_string_UART0(sx);
// transmit_UART0(' ');
// transmit_UART0('\r');
//
// sprintf(sy,"%d",temp[1]);
// transmit_UART0('M');
// transmit_string_UART0(sy);
// transmit_UART0(' ');
// transmit_UART0('\r');
//
// sprintf(sz,"%d",temp[2]);
// transmit_UART0('N');
// transmit_string_UART0(sz);
// transmit_UART0(' ');
// transmit_UART0('\r');
// sprintf(sz,"%d",temp);
// transmit_UART0('M');
// transmit_string_UART0(sz);
// transmit_UART0(' ');
// transmit_UART0('\r');
// Current_state.pwm.x_dir = 0;
// Current_state.pwm.y_dir = 0;
// Current_state.pwm.z_dir = 0;
/// Transmit Torquer Current Data to the Terminal
sprintf(sx,"%d",Current_state.pwm.x);
transmit_UART0('X');
transmit_string_UART0(sx);
transmit_UART0(' ');
sprintf(sy,"%d",Current_state.pwm.y);
transmit_UART0('Y');
transmit_string_UART0(sy);
transmit_UART0(' ');
sprintf(sz,"%d",Current_state.pwm.z);
transmit_UART0('Z');
transmit_string_UART0(sz);
transmit_UART0(' ');
transmit_UART0('\r');
//
}
}
return 0;
}
void back_L(int power)
{
set_PWM(LEF0, 0);
set_PWM(LEF1, power);
}
void PWM_State_machine() {
button0 = ~PINA & 0x01; //connects PA0 to A0
button1 = ~PINA & 0x02; //connects PA1 to A1
button2 = ~PINA & 0x04; //connects PA1 to A2
switch(SM1_State) { // Transitions
case init:
if(button0 && !button1 && !button2)
{
SM1_State = c4;
}
else if(!button0 && button1 && !button2)
{
SM1_State = d4;
}
else if(!button0 && !button1 && button2)
{
SM1_State = e4;
}
else {
SM1_State = init;
}
break;
case c4:
if(button0 && !button1 && !button2)
{
SM1_State = c4;
}
else
{
SM1_State = init;
}
break;
case d4:
if(!button0 && button1 && !button2)
{
SM1_State = d4;
}
else
{
SM1_State = init;
}
break;
case e4:
if(!button0 && !button1 && button2)
{
SM1_State = e4;
}
else
{
SM1_State = init;
}
break;
default:
SM1_State = init;
} // Transitions
switch(SM1_State) { // State actions
case init:
set_PWM(0);
break;
case c4:
set_PWM(261.63);
break;
case d4:
set_PWM(293.66);
break;
case e4:
set_PWM(329.63);
break;
default: // ADD default behaviour below
break;
} // State actions
}
int main(void)
{
/// Define Variables to store Magnetic field
char array[40];
char array1[40];
char array2[40];
char array3[40];
int16_t Bx;
int16_t By;
int16_t Bz;
/// Blink LED to show that program is successfully running
DDRA = 0xF0;
PORTA = 0xF0;
_delay_ms(1000);
PORTA = 0x00;
_delay_ms(1000);
PORTA = 0xF0;
_delay_ms(1000);
PORTA = 0x00;
_delay_ms(1000);
/// Inittialise UART0 for Transmission to terminal
init_UART0();
// init_UART1();
//
/// Transmit "Hello" String
transmit_UART0('\r');
transmit_UART0('\r');
transmit_UART0('H');
transmit_UART0('e');
transmit_UART0('l');
transmit_UART0('l');
transmit_UART0('o');
sprintf(array,"\t..This is IITB's Student Satellite...\r");
transmit_string_UART0(array);
sprintf(array1,"\tThis is PRATHAM's OBC-Master code...");
sprintf(array2,"\rCurrent MagnetoMeter state is =\t");
sprintf(array3,"\rGenerating Torquer Current for =\t");
transmit_string_UART0(array1);
// transmit_string_UART0(array2);
// transmit_string_UART0(array3);
/// Initialise UART1 for Magnetometer Reception of Data and Transmission of Poll
init_UART_MM();
/// Configure the torquer to output the required current values
configure_torquer();
/// Define 3 strings for storing Magnetometer field values
char sx[2];
char sy[2];
char sz[2];
float A[4] = {1,0.75,0.50,0.25};
timer_init();
/// Start while loop
while (1)
{
/// Receive and store Bx,By,Bz
Bx=(int16_t)receive_MM();
Bx=(Bx<<8);
Bx &= 0xFF00;
Bx|=(int16_t)receive_MM();
By=(int16_t)receive_MM();
By=(By<<8);
By &= 0xFF00;
By|=(int16_t)receive_MM();
Bz=(int16_t)receive_MM();
Bz=(Bz<<8);
Bz &= 0xFF00;
Bz|=(int16_t)receive_MM();
/// Receive carriage return and ignore
receive_MM();
/// Transmit Carriage return
transmit_UART0('\r');
if (Bx != 0x00 || By != 0x00 || Bz != 0x00)
{
transmit_UART0('z');
transmit_string_UART0(sz);
transmit_UART0(' ');
sprintf(sz,"%d",By);
transmit_UART0('y');
transmit_string_UART0(sz);
transmit_UART0(' ');
sprintf(sz,"%d",Bx);
transmit_UART0('x');
transmit_string_UART0(sz);
transmit_UART0(' ');
transmit_UART0('\r');
transmit_string_UART0(array3);
transmit_UART0(' ');
transmit_UART0('X');
transmit_UART0(' ');
transmit_UART0('Y');
transmit_UART0(' ');
transmit_UART0('Z');
transmit_UART0('\r');
if (dir==0)
dir=1;
else
dir=0;
Current_state.pwm.x_dir = dir;
Current_state.pwm.x = 32768*2*Bx/5225;
Current_state.pwm.y_dir = dir;
Current_state.pwm.y = 32768*By*2/5225;
Current_state.pwm.z_dir = dir;
Current_state.pwm.z = 32768*Bz*2/5225;
set_PWM();
}
}
return 0;
}
void stop_motor_L(void)//Turn off left motor
{
set_PWM(LEF0, 0);
set_PWM(LEF1, 0);
}
int main(void){
///Configure the Torquer
configure_torquer();
_delay_ms(1000);
/// Initialise Interfaces - UART of Magnetometer and GPS and the SPI bus
init_UART_MM();
init_UART_GPS();
init_SPI();
init_TWI();
///Set Preflight pin as input
cbi(DDR_PF, PIN_PF);
///* Switch on Global interrupts
sei();
///Check if Preflight Pin is high
while(1){
///* * Reset timer for 2 seconds
timer_reset_two_sec();
///* Get Health Monitoring data
get_HM_data();
///* Preflight Checks
if(PORT_PF & _BV(PIN_PF)){
///* * Set the mode as preflight
Mode = PREFLIGHT;
send_preflight("Master\r", 7);
///* * GPS test
read_GPS();
while(UCSR0B & _BV(RXCIE0));
send_preflight((char *)&Current_state.gps, sizeof(struct GPS_reading));
///* * Magnetometer and Torquer test
///* * Reading with no torquers on
read_MM ();
send_preflight((char *)&Current_state.mm, sizeof(struct MM_reading));
///* * Reading with one torquer on at once
Current_state.pwm.x_dir = 0;
Current_state.pwm.x = 32768;
Current_state.pwm.y_dir = 0;
Current_state.pwm.y = 0;
Current_state.pwm.z_dir = 0;
Current_state.pwm.z = 0;
set_PWM ();
read_MM ();
send_preflight((char *)&Current_state.mm, sizeof(struct MM_reading));
Current_state.pwm.x_dir = 0;
Current_state.pwm.x = 0;
Current_state.pwm.y_dir = 0;
Current_state.pwm.y = 32768;
Current_state.pwm.z_dir = 0;
Current_state.pwm.z = 0;
set_PWM ();
read_MM ();
send_preflight((char *)&Current_state.mm, sizeof(struct MM_reading));
Current_state.pwm.x_dir = 0;
Current_state.pwm.x = 0;
Current_state.pwm.y_dir = 0;
Current_state.pwm.y = 0;
Current_state.pwm.z_dir = 0;
Current_state.pwm.z = 32768;
set_PWM ();
read_MM ();
send_preflight((char *)&Current_state.mm, sizeof(struct MM_reading));
///* * Reading with one torquer on at once, in other direction
Current_state.pwm.x_dir = 1;
Current_state.pwm.x = 32768;
Current_state.pwm.y_dir = 0;
Current_state.pwm.y = 0;
Current_state.pwm.z_dir = 0;
Current_state.pwm.z = 0;
set_PWM ();
read_MM ();
send_preflight((char *)&Current_state.mm, sizeof(struct MM_reading));
Current_state.pwm.x_dir = 0;
Current_state.pwm.x = 0;
Current_state.pwm.y_dir = 1;
Current_state.pwm.y = 32768;
Current_state.pwm.z_dir = 0;
Current_state.pwm.z = 0;
set_PWM ();
read_MM ();
send_preflight((char *)&Current_state.mm, sizeof(struct MM_reading));
Current_state.pwm.x_dir = 0;
Current_state.pwm.x = 0;
Current_state.pwm.y_dir = 0;
Current_state.pwm.y = 0;
Current_state.pwm.z_dir = 1;
Current_state.pwm.z = 32768;
set_PWM ();
read_MM ();
send_preflight((char *)&Current_state.mm, sizeof(struct MM_reading));
///* * Set Torquer values to zero
reset_PWM();
///* * Sunsensor test
read_SS();
send_preflight((char *)&Current_state.ss, sizeof(struct SS_reading));
///* Health Montoring
get_HM_data();
send_preflight((char *)&Current_state.hm, sizeof(struct HM_data));
///Communication Task
comm();
///* * Wait for 2 seconds to get over
timer_wait_reset();
}
///Normal Mode
else{
///* Set default mode of Satellite
Mode = DETUMBLING;
///* initialise Timer
Time = 0;
///Loop begins
while(!(PORT_PF & _BV(PIN_PF))){
/**
* * * * Task 2: Control codes
* @ref control
*/
control();
/**
* * * * Task 1: Communication with power uC through I2C. @ref power
*/
power();
/**
* * * * Task 3: Communication check routine;
* @ref comm
*/
comm();
wdt_disable();
///* * Increment the Timer
Time += FRAME_TIME;
///* * Wait for 2 seconds to get over
timer_wait_reset();
}
}
}
return 0;
}
void playSound(){
static double currFreq;
static unsigned char soundTime = 0;
soundTime++;
if(soundTime == 10){
soundTime = 0;
switch(soundStatus){
case none:
currFreq = 0;
set_PWM(currFreq);
break;
case normal:
if(currFreq == 0){
currFreq = 261.63;
set_PWM(currFreq);
}
else if(currFreq == 261.63){
currFreq = 293.66;
set_PWM(currFreq);
}
else if(currFreq == 293.66){
currFreq = 329.63;
set_PWM(currFreq);
}
else if(currFreq == 329.63){
currFreq = 0;
set_PWM(currFreq);
soundStatus = none;
}
break;
case special:
if(currFreq == 0){
currFreq = 261.63;
set_PWM(currFreq);
}
else if(currFreq == 261.63){
currFreq = 293.66;
set_PWM(currFreq);
}
else if(currFreq == 293.66){
currFreq = 329.63;
set_PWM(currFreq);
}
else if(currFreq == 329.63){
currFreq = 0;
set_PWM(currFreq);
soundStatus = none;
}
break;
case bad:
if(currFreq == 0){
currFreq = 261.63;
set_PWM(currFreq);
}
else if(currFreq == 261.63){
currFreq = 293.66;
set_PWM(currFreq);
}
else if(currFreq == 293.66){
currFreq = 329.63;
set_PWM(currFreq);
}
else if(currFreq == 329.63){
currFreq = 0;
set_PWM(currFreq);
soundStatus = none;
}
break;
}
}
}
void stop_motor_R(void)//Turn off right motor
{
set_PWM(RIG0, 0);
set_PWM(RIG1, 0);
}
void ahead_L(int power)
{
set_PWM(LEF0, power);
set_PWM(LEF1, 0);
}
void ahead_R(int power)
{
set_PWM(RIG0, power);
set_PWM(RIG1, 0);
}
void back_R(int power)
{
set_PWM(RIG0, 0);
set_PWM(RIG1, power);
}
int sound_tick(int state){
switch(state){
case st_sound_start:
state = st_sound_init;
break;
case st_sound_init:
state = st_sound_idle;
break;
case st_sound_idle:
if(!QueueIsEmpty(sound_command_queue)){
if(sound_command_queue->buffer[sound_command_queue->front] == snd_locked){
state = st_sound_lock1;
} else if(sound_command_queue->buffer[sound_command_queue->front] == snd_unlocked){
state = st_sound_unlock1;
} else if(sound_command_queue->buffer[sound_command_queue->front] == snd_malfunction){
state = st_sound_malfunction1;
}
QueueDequeue(sound_command_queue);
} else {
state = st_sound_idle;
}
break;
case st_sound_lock1:
state = st_sound_lock2;
break;
case st_sound_lock2:
state = st_sound_lock3;
break;
case st_sound_lock3:
state = st_sound_lock4;
break;
case st_sound_lock4:
state = st_sound_idle;
break;
case st_sound_unlock1:
state = st_sound_unlock2;
break;
case st_sound_unlock2:
state = st_sound_idle;
break;
case st_sound_malfunction1:
state = st_sound_malfunction2;
break;
case st_sound_malfunction2:
state = st_sound_malfunction3;
break;
case st_sound_malfunction3:
state = st_sound_malfunction4;
break;
case st_sound_malfunction4:
if(!QueueIsEmpty(sound_command_queue)){
state = st_sound_idle;
} else {
state = st_sound_malfunction1;
}
break;
default:
state = st_sound_start;
break;
}
switch(state){
case st_sound_init:
init_PWM();
break;
case st_sound_lock1:
set_PWM(3135.96);
break;
case st_sound_lock2:
set_PWM(0);
break;
case st_sound_lock3:
set_PWM(3135.96);
break;
case st_sound_lock4:
set_PWM(0);
break;
case st_sound_unlock1:
set_PWM(3135.96);
break;
case st_sound_unlock2:
set_PWM(0);
break;
case st_sound_malfunction1:
set_PWM(130.81);
PORTD |= 0x02;
break;
case st_sound_malfunction3:
set_PWM(0);
PORTD &= 0xFD;
break;
default:
break;
}
return state;
}
void Tick()
{
switch(state) // transitions
{
case Start:
{
state = INIT;
break;
}
case INIT:
{
if((~PINA & 0x07) != 0x00)
{
state = ON;
break;
}
else
{
state = INIT;
break;
}
}
case ON:
{
if((~PINA & 0x07) != 0x00)
{
state = ON; break;
}
else if((~PINA & 0x07) == 0x03)
{
state = OFF; break;
}
else if((~PINA & 0x07) == 0x05)
{
state = OFF; break;
}
else if((~PINA & 0x07) == 0x06)
{
state = OFF; break;
}
else if((~PINA & 0x07) == 0x07)
{
state = OFF; break;
}
else
{
state = OFF; break;
}
}
case OFF:
{
state = INIT;
break;
}
default:
break;
}
switch(state)
{
case Start:
break;
case INIT:
{
set_PWM(0);
break;
}
case ON:
{
if((~PINA & 0x07) == 0x01)
{
set_PWM(261.63);
break;
}
else if((~PINA & 0x07) == 0x02)
{
set_PWM(293.66);
break;
}
else if((~PINA & 0x07) == 0x04)
{
set_PWM(329.63);
break;
}
}
case OFF:
{
set_PWM(0);
break;
}
default:
break;
}
}
