int main() {
pc.baud(115200);
float temp;
pc.printf("Hello!");
//implement XBee communication
send.attach(&sendPackage,0.7);
//implement EMG signal processing
adc_aqr.attach(&adc_acquire,0.05);
adc_aqr_temp.attach(&adc_acquire_temp,0.05);
get_position.attach(&position_decide,0.05);
get_position_temp.attach(&position_decide_temp,0.05);
xbee.baud(57600);
rst = 0;
led1 = 0;
wait_ms(1);
rst = 1;
wait_ms(1);
led1 = 1;
led2 = 0;
// xbee send data
msg.id = 0;
msg.mode_send = 1;
msgIface.sendPacket( MsgTypeModeSend, (uint8_t*)&msg, sizeof(MsgModeSend) );
while ( !msgIface.sendComplete() ) {
msgIface.sendNow();
}
wait_ms(250);
led2 = 1;
wait_ms(250);
led2 = 0;
msg.id = 0;
msg.mode_send = 1;
msgIface.sendPacket( MsgTypeModeSend, (uint8_t*)&msg, sizeof(MsgModeSend) );
while ( !msgIface.sendComplete() ) {
msgIface.sendNow();
}
wait_ms(250);
led2 = 1;
wait_ms(250);
led2 = 0;
msg.id = 0;
msg.mode_send = 1;
msgIface.sendPacket( MsgTypeModeSend, (uint8_t*)&msg, sizeof(MsgModeSend) );
//Implement IMU function
imu_mpu6050.setAcceleroRange(MPU6050_ACCELERO_RANGE_2G); // set the scale of the accelerometer
imu_mpu6050.setGyroRange(MPU6050_GYRO_RANGE_250); // set the scale of the gyro
float angle_x;
float angle_y; // store the angle of x-axis and angle of y-axis
bool success_flag; // indicate the iic connection
success_flag = imu_hand.testCommunication(); // if successful, return 1
//train and get the train sample
init();
while(!success_flag) // wait until connection
{
myled = 1;
}
// while loop
t.start();
while(1) {
imu_hand.getAngleXY(&angle_x, &angle_y); // get angle_x and angle_y
myQueue.Put(&adc);
myQueue_temp.Put(&adc_temp);
//pc.printf("y: %f",angle_y);
if(pos==2&&pos==2&& angle_y>65)
{
if(mode_enable==1)
{
t.reset();
mode_enable=0;
pc.printf("SHOOT\r\n");
msg.id = 0;
msg.mode_send = SHOOT;
}
}
// pc.printf("time : %f\r\n", t.read());
// pc.printf("mode_enable : %d\r\n", mode_enable);
if(t.read()>3)
{
t.reset();
mode_enable=1;
}
if(pos==0&&pos_temp==0)
{
pc.printf("IDLE\r\n");
msg.id = 0;
msg.mode_send = 0;
}
else
{
if(adc-adc_temp>120)
{
if(abs(angle_x)<30)
{
pc.printf("CANNON_UP\r\n");
msg.id = 0;
msg.mode_send = CANNON_UP;
}
if(angle_x>60 && angle_x<90)
{
pc.printf("CANNON_LEFT\r\n");
msg.id = 0;
msg.mode_send = CANNON_LEFT;
}
if(angle_x<-60 && angle_x>-90)
{
pc.printf("SHOOT\r\n");
msg.id = 0;
msg.mode_send = SHOOT;
}
}
else if(adc_temp-adc>-100)
{
if(abs(angle_x)<30)
{
pc.printf("CANNON_DOWN\r\n");
msg.id = 0;
msg.mode_send = CANNON_DOWN;
}
if(angle_x>60 && angle_x<90)
{
pc.printf("CANNON_RIGHT\r\n");
msg.id = 0;
msg.mode_send = CANNON_RIGHT;
}
}
}
// pc.printf("pos: %d\r\n",pos_temp);
if(myQueue.GetNumberOfItems( )==sample_num)
{
myQueue.Get(&temp);
}
if(myQueue_temp.GetNumberOfItems( )==sample_num)
{
myQueue_temp.Get(&temp);
}
msgIface.sendNow();
}
}
