//--------------------------------------------------------------
main()
{
Buzzer* Prog;
Prog = new Buzzer;
Prog->Run();
return 0;
}
int main(int argc, char **argv) {
bool prevsynced=true;
while (true) {
Task::sleep(5);
if (vex.getSynced()) {
VexRC::Channels chans = vex.getChannels();
out.printf("%d %d %d %d ", chans.analogs[0], chans.analogs[1], chans.analogs[2], chans.analogs[3]);
out.printf("%s %s\n", digitalToString(chans.left), digitalToString(chans.right));
prevsynced = true;
} else {
if (prevsynced) {
buzzer.buzz(100);
out.print("Not synced\n");
}
prevsynced = false;
}
}
}
webInterfaceHandler() {
cout << "\033[36m[THRIFT]\033[0m Initialising hexacopter systems." << endl;
if (!initialise(&fb, &gps, &imu, &cam)) terminate();
buzzer.playBuzzer(0.4, 100, 100);
}
int main(int argc, char **argv) {
imu.start();
out.printf("Battery: %f cell\n", batmon.getCellVoltage());
out.print("Waiting for remote signal\n");
while (!vex.getSynced()) { Task::sleep(100); }
out.print("Arming\n");
motors.arm();
float heading = imu.getYaw();
buzzer.buzz(300);
while (true) {
control.start();
Logger logger(eeprom, 0);
bool logging = false;
while (true) {
bool up = false;
if (!vex.getSynced()) {
control.stop();
motors.off();
while (!vex.getSynced()) { Task::sleep(100); }
control.start();
sensors.centerGyros();
heading = imu.getYaw();
buzzer.buzz(300);
}
VexRC::Channels chans = vex.getChannels();
if (chans.right != VexRC::NONE)
break;
if (chans.left == VexRC::UP)
logging = true;
else if (chans.left == VexRC::DOWN)
logging = false;
float throttle = chans.analogs[1] / 50.0;
if (throttle < 0)
throttle = 0;
float rollsetpoint = (-chans.analogs[3] / 50.0) * 0.3;
float pitchsetpoint = (-chans.analogs[2] / 50.0) * 0.3;
heading += (chans.analogs[0] / 50.0) * (M_PI / 4) * .005;
control.setControlPoints(throttle, rollsetpoint, pitchsetpoint, heading);
if (!up) {
if (throttle > 0.4)
up = true;
else
control.clearIntegrals();
}
if (logging) {
LogEntry entry = { sensors.getReadings(), imu.getState(), throttle };
logger.write((uint8_t *)&entry, sizeof(entry));
}
IMU::State state = imu.getState();
IMU::State velstate = imu.getVelocityState();
out.printf("%f %f\n", control.getRollCorrection(), control.getPitchCorrection());
Task::sleep(25);
}
control.stop();
motors.off();
out.print("Push enter\n");
while (out.getch() != '\r') { }
out.print("Press y to dump log");
if (out.getch() == 'y') {
out.print("\nLog dump:");
LogReader reader(eeprom, 0);
struct LogEntry entry;
while (reader.read((uint8_t *)&entry, sizeof(entry)) == sizeof(entry)) {
int i;
for (i=0;i<6;i++) {
out.printf("%.3f ", entry.analogs.array[i]);
}
out.printf("%.3f %.3f %.3f ", entry.state.roll, entry.state.pitch, entry.state.yaw);
out.printf("%.3f\n", entry.throttle);
}
}
docalibrate("Roll", &controlconfig.roll_config);
docalibrate("Pitch", &controlconfig.pitch_config);
docalibrate("Yaw", &controlconfig.yaw_config);
out.printf("Current stick values: %f %f\n", controlconfig.rollpitch_stickfactor, controlconfig.yaw_stickfactor);
out.printf("Stick: ");
static char buf[50];
out.getline(buf, sizeof(buf));
sscanf(buf, "%f %f", &controlconfig.rollpitch_stickfactor, &controlconfig.yaw_stickfactor);
}
}
/*
* flightLoop
* 1) Initialise copter systems
* 2) Wait for user allowance to fly.
* 3) Read in list of waypoints
* 4) Fly to first waypoint, wait, fly to next, etc..
* 5) If the end is reached, stop.
*
* The user may stop the flight at any time. It will continue if the user resumes. At any
* point, the user may stop the current flight path and change the waypoint configuration. Upon
* resuming flight, the copter will read in the new list of waypoints and start at the
* beginning.
*/
void waypointsFlightLoop(FlightBoard &fb, GPS &gps, IMU &imu, Buzzer &buzzer, Logger &logs, deque<coord> &waypoints_list) {
cout << "\033[1;32m[WAYPTS]\033[0m Waypoints thread initiated, travelling to the following waypoints:" << endl;
char str_buf[BUFSIZ];
bool useimu = false; // Manual setting for debug
double yaw = 0;
GPS_Data gps_data;
for(size_t i = 0; i != waypoints_list.size(); i++) {
cout << ' ' << i+1 << ": (" << waypoints_list[i].lat << ", " << waypoints_list[i].lon << ")" << endl;
}
state = 6;
while ( !gpio::isAutoMode() ) usleep(1000000);
/*if (!useimu) {
buzzer.playBuzzer(0.25, 100, 100);
state = 5;
yaw = inferBearing(&fb, &gps, &logs);
}*/
coord currentCoord = {-1, -1};
double distanceToNextWaypoint;
double bearingToNextWaypoint;
FB_Data course = {0, 0, 0, 0};
int pastState = -1;
try { // Check for any errors, and stop the copter.
while(!exitProgram) {
gps.getGPS_Data(&gps_data);
currentCoord.lat = gps_data.latitude;
currentCoord.lon = gps_data.longitude;
if (wp_it == waypoints_list.size()) {
wp_it = 0;
userState = 0;
}
distanceToNextWaypoint = calculate_distance(currentCoord, waypoints_list[wp_it]);
bearingToNextWaypoint = calculate_bearing(currentCoord, waypoints_list[wp_it]);
//if (useimu) yaw = getYaw(&imu);
yaw = gps_data.heading;
/* State 4: Manual mode. */
if (!gpio::isAutoMode()) {
state = 4;
wp_it = 0;
exitProgram = true;
/* State 0: All stop */
} else if (waypoints_list.empty() || userState == 0 ) {
state = 11;
wp_it = 0;
exitProgram = true;
/* State 3: Error. */
} else if (state == 3 || !checkInPerth(¤tCoord)) {
state = 3;
exitProgram = true;
/* State 2: At waypoint. */
} else if (distanceToNextWaypoint < WAYPOINT_RADIUS) {
state = 2;
/* State 1: Travelling to waypoint. */
} else {
state = 1;
}
/* Only give output if the state changes. Less spamey.. */
if (pastState != state) {
switch (state) {
case 0:
cout << "\033[1;32m[WAYPTS]\033[0m All stop." << endl;
break;
case 1:
cout << "\033[1;32m[WAYPTS]\033[0m Travelling to waypoint " << wp_it << ", at (" << waypoints_list[wp_it].lat << "," << waypoints_list[wp_it].lon << ")" << endl;
break;
case 2:
cout << "\033[1;32m[WAYPTS]\033[0m At waypoint" << wp_it << "." << endl;
break;
case 3:
cout << "\033[31m[WAYPTS]\033[0m Error reading GPS." << endl;
case 4:
cout << "\033[31m[WAYPTS]\033[0m Manual mode engaged." << endl;
break;
}
cout << "\033[1;33m[WAYPTS]\033[0m In state " << state << "." << endl;
cout << "\033[1;33m[WAYPTS]\033[0m Facing " << setprecision(6) << yaw << ", at coordinates (" << currentCoord.lat << ", " << currentCoord.lon << ")" << endl;
cout << "\033[1;33m[WAYPTS]\033[0m The next waypoint is at (" << setprecision(6) << waypoints_list[wp_it].lat << ", " << waypoints_list[wp_it].lon << ")" << endl;
cout << "\033[1;33m[WAYPTS]\033[0m It is " << setprecision(7) << distanceToNextWaypoint << "m away, at a bearing of " << bearingToNextWaypoint << "." << endl;
printFB_Data(&stop);
pastState = state;
}
switch(state) {
case 0: //Case 0: Not in auto mode, standby
fb.setFB_Data(&stop); //Stop moving
logs.writeLogLine("\033[1;32m[WAYPTS]\033[0m Manual mode");
sprintf(str_buf, "[WAYPTS] Currently at %f %f.", currentCoord.lat, currentCoord.lon);
logs.writeLogLine(str_buf);
break;
case 1:
bearingToNextWaypoint = calculate_bearing(currentCoord, waypoints_list[wp_it]); //Set a Course
setCourse(&course, distanceToNextWaypoint, bearingToNextWaypoint, yaw);
fb.setFB_Data(&course); //Give command to flight boars
sprintf(str_buf, "[WAYPTS] Aileron is %d, Elevator is %d", course.aileron, course.elevator);
logs.writeLogLine(str_buf);
sprintf(str_buf, "[WAYPTS] Moving to next waypoint. It has latitude %f and longitude %f.", waypoints_list[wp_it].lat, waypoints_list[wp_it].lon);
logs.writeLogLine(str_buf);
sprintf(str_buf, "[WAYPTS] Currently at %f %f, moving %f m at a bearing of %f degrees.", currentCoord.lat, currentCoord.lon, distanceToNextWaypoint, bearingToNextWaypoint);
logs.writeLogLine(str_buf);
break;
case 2:
fb.setFB_Data(&stop);
buzzer.playBuzzer(0.4, 100, 100);
logs.writeLogLine("\033[1;32m[WAYPTS]\033[0m Reached waypoint, stopping");
//waypoints_list.push_back(waypoints_list.front());
wp_it++;
//waypoints_list.pop_front();
boost::this_thread::sleep(boost::posix_time::milliseconds(WAIT_AT_WAYPOINTS));
cout << "\033[1;32m[WAYPTS]\033[0m Moving to next waypoint." << endl;
break;
case 3:
default:
fb.setFB_Data(&stop);
logs.writeLogLine("\033[31m[WAYPTS]\033[0m Error reading GPS, stopping");
break;
}
boost::this_thread::sleep(boost::posix_time::milliseconds(100));
}
} catch (...) {
cout << "\033[31m[WAYPTS]\033[0m Error encountered. Stopping copter." << endl;
fb.setFB_Data(&stop);
printFB_Data(&stop);
state = 3;
}
cout << "\033[1;32m[WAYPTS]\033[0m Waypoints flight loop terminating." << endl;
buzzer.playBuzzer(0.2, 100, 100);
usleep(400000);
buzzer.playBuzzer(0.2, 100, 100);
usleep(400000);
buzzer.playBuzzer(0.2, 100, 100);
usleep(400000);
}
/*
* flightLoop
* 1) Initialise copter systems
* 2) Wait for user allowance to fly.
* 3) Read in list of waypoints
* 4) Fly to first waypoint, wait, fly to next, etc..
* 5) If the end is reached, stop.
*
* The user may stop the flight at any time. It will continue if the user resumes. At any
* point, the user may stop the current flight path and change the waypoint configuration. Upon
* resuming flight, the copter will read in the new list of waypoints and start at the
* beginning.
*/
void waypoints_loop4(hardware &hardware_list, Logger &log, deque<coord> &waypoints_list, string config_filename) {
cout << "\033[1;32m[WAYPTS]\033[0m Waypoints thread initiated, travelling to the following waypoints:" << endl;
char str_buf[BUFSIZ];
log.clearLog();
time_t start, now, last_displayed;
time(&start);
time(&now);
time(&last_displayed);
//Grab references to hardware
FlightBoard *fb = hardware_list.fb;
GPS *gps = hardware_list.gps;
IMU *imu = hardware_list.imu;
bool useimu = hardware_list.IMU_Working;
double yaw = 0;
//Configure configurable variables (if file is given)
if(!config_filename.empty()) {
loadParameters(config_filename);
}
//Construct PID controller
PID controller_perp = PID(-Kp_PERP, -Ki_PERP, -Kd_PERP, MAIN_LOOP_DELAY, 3, 0.95);
PID controller_inline = PID(-Kp_INLINE, -Ki_INLINE, -Kd_INLINE, MAIN_LOOP_DELAY, 3, 0.95);
//Construct buzzer
Buzzer buzzer;
buzzer.playBuzzer(BUZZER_DURATION, BUZZER_FREQUENCY, BUZZER_VOLUME);
usleep((int)(BUZZER_DURATION*1.1)*1000*1000);
//Print list of waypoints
for(size_t i = 0; i != waypoints_list.size(); i++) {
cout << " " << i+1 << ": (" << setprecision(6) << waypoints_list[i].lat << ", " << setprecision(6) << waypoints_list[i].lon << ")" << endl;
}
//Wait here for auto mode and conduct bearing test if necessary
state = 6;
cout << "\033[1;33m[WAYPTS]\033[0m Waiting for auto mode." << endl;
while ( !gpio::isAutoMode() && !exitProgram) usleep(500*1000);
cout << "\033[1;31m[WAYPTS]\033[0m Auto mode engaged." << endl;
if (!useimu && !exitProgram) {
buzzer.playBuzzer(0.25, 10, 100);
cout << "\033[1;31m[WAYPTS]\033[0m Conducting bearing test..." << endl;
state = 5;
yaw = inferBearing(fb, gps);
cout << "\033[1;31m[WAYPTS]\033[0m Bearing test complete." << endl;
}
state = 1;
//Initialise loop variables
coord currentCoord = {-1, -1};
double distanceToNextWaypoint;
double bearingToNextWaypoint;
FB_Data course = {0, 0, 0, 0};
int pastState = -1;
velocity flightVector = {-1, -1};
//Plot initial path
currentCoord = getCoord(gps);
line path;
if (!waypoints_list.empty()) {
path = get_path(currentCoord, waypoints_list[wp_it]);
}
cout << "\033[1;32m[WAYPTS]\033[0m Starting main loop." << endl;
try { // Check for any errors, and stop the copter.
while(!exitProgram) {
currentCoord = getCoord(gps);
//Write data for Michael
time(&now);
if (!waypoints_list.empty()) {
sprintf(str_buf, "%.f,%3.6f,%3.6f,%3.6f,%3.6f", difftime(now, start), currentCoord.lat, currentCoord.lon, waypoints_list[wp_it].lat, waypoints_list[wp_it].lon);
} else {
sprintf(str_buf, "%.f,%3.6f,%3.6f,,", difftime(now, start), currentCoord.lat, currentCoord.lon);
}
log.writeLogLine(str_buf, false);
if(!waypoints_list.empty()) {
distanceToNextWaypoint = calculate_distance(currentCoord, waypoints_list[wp_it]);
bearingToNextWaypoint = calculate_bearing(currentCoord, waypoints_list[wp_it]);
}
if (useimu) yaw = getYaw(imu);
/* State 4: Manual mode. */
if (!gpio::isAutoMode()) {
state = 4;
wp_it = 0;
exitProgram = true;
/* State 0: All stop */
} else if (waypoints_list.empty() || wp_it == waypoints_list.size() || userState == 0 ) {
state = 11;
userState = 0;
wp_it = 0;
exitProgram = true;
/* State 3: Error. */
} else if (state == 3 || !checkInPerth(¤tCoord)) {
state = 3;
exitProgram = true;
/* State 2: At waypoint. */
} else if (distanceToNextWaypoint < WAYPOINT_RADIUS) {
state = 2;
/* State 1: Travelling to waypoint. */
} else {
state = 1;
}
/* Only give output if the state changes. Less spamey.. */
if (pastState != state || (state == 1 && difftime(now, last_displayed) >0.9)) {
switch (state) {
case 0:
cout << "\033[1;32m[WAYPTS]\033[0m All stop." << endl;
break;
case 1:
cout << "\033[1;32m[WAYPTS]\033[0m Travelling to waypoint " << wp_it << ", at (" << waypoints_list[wp_it].lat << "," << waypoints_list[wp_it].lon << ")" << endl;
break;
case 2:
cout << "\033[1;32m[WAYPTS]\033[0m At waypoint" << wp_it << "." << endl;
break;
case 3:
cout << "\033[31m[WAYPTS]\033[0m Error reading GPS." << endl;
case 4:
cout << "\033[31m[WAYPTS]\033[0m Manual mode engaged." << endl;
break;
}
cout << "\033[1;33m[WAYPTS]\033[0m In state " << state << "." << endl;
cout << "\033[1;33m[WAYPTS]\033[0m Facing " << setprecision(6) << yaw << ", at coordinates (" << currentCoord.lat << ", " << currentCoord.lon << ")" << endl;
cout << "\033[1;33m[WAYPTS]\033[0m The next waypoint is at (" << setprecision(6) << waypoints_list[wp_it].lat << ", " << waypoints_list[wp_it].lon << ")" << endl;
cout << "\033[1;33m[WAYPTS]\033[0m It is " << setprecision(7) << distanceToNextWaypoint << "m away, at a bearing of " << bearingToNextWaypoint << "." << endl;
pastState = state;
last_displayed = now;
}
switch(state) {
case 0: //Case 0: Not in auto mode, standby
fb->setFB_Data(&stop); //Stop moving
/*
log.writeLogLine("\033[1;32m[WAYPTS]\033[0m Manual mode");
sprintf(str_buf, "[WAYPTS] Currently at %f %f.", currentCoord.lat, currentCoord.lon);
log.writeLogLine(str_buf);
*/
break;
case 1:
flightVector = get_velocity(&controller_perp, &controller_inline, currentCoord, path, SPEED_LIMIT_);
setCourse(&course, flightVector, yaw);
fb->setFB_Data(&course); //Give command to flight boars
/*
sprintf(str_buf, "[WAYPTS] Aileron is %d, Elevator is %d", course.aileron, course.elevator);
log.writeLogLine(str_buf);
sprintf(str_buf, "[WAYPTS] Moving to next waypoint. It has latitude %f and longitude %f.", waypoints_list[wp_it].lat, waypoints_list[wp_it].lon);
log.writeLogLine(str_buf);
sprintf(str_buf, "[WAYPTS] Currently at %f %f, moving %f m at a bearing of %f degrees.", currentCoord.lat, currentCoord.lon, distanceToNextWaypoint, bearingToNextWaypoint);
log.writeLogLine(str_buf);
*/
break;
case 2:
fb->setFB_Data(&stop);
buzzer.playBuzzer(BUZZER_DURATION, BUZZER_FREQUENCY, BUZZER_VOLUME);
/*
log.writeLogLine("\033[1;32m[WAYPTS]\033[0m Reached waypoint, stopping");
*/
wp_it++;
if(repeatLoop && wp_it == waypoints_list.size()) {
wp_it = 0;
}
if (wp_it != waypoints_list.size()) {
path = get_path(currentCoord, waypoints_list[wp_it]);
}
usleep(WAIT_AT_WAYPOINTS*1000);
controller_perp.clear();
controller_inline.clear();
cout << "\033[1;32m[WAYPTS]\033[0m Moving to next waypoint." << endl;
break;
case 3:
default:
fb->setFB_Data(&stop);
/*
log.writeLogLine("\033[31m[WAYPTS]\033[0m Error reading GPS, stopping");
*/
break;
}
usleep(MAIN_LOOP_DELAY*1000);
}
} catch (...) {
cout << "\033[31m[WAYPTS]\033[0m Error encountered. Stopping copter." << endl;
fb->setFB_Data(&stop);
state = 3;
}
cout << "\033[1;32m[WAYPTS]\033[0m Waypoints flight loop terminating." << endl;
buzzer.playBuzzer(1.0, 20, 60);
usleep(2100*1000);
}
int main(void)
{
Led0 led0;led0.setupDigitalOut();
Led1 led1;led1.setupDigitalOut();
Led2 led2;led2.setupDigitalOut();
Led3 led3;led3.setupDigitalOut();
Buzzer buzzer;buzzer.setupDigitalOut();
Sw0 sw0;sw0.setupDigitalIn();
Sw1 sw1;sw1.setupDigitalIn();
Sw2 sw2;sw2.setupDigitalIn();
Sw3 sw3;sw3.setupDigitalIn();
A0 a0;//a0.setupAnalogIn();
A1 a1;a1.setupAnalogIn();
A2 a2;//a2.setupAnalogIn();
A3 a3;//a3.setupAnalogIn();
A4 a4;a4.setupAnalogIn();
/*Init_ADC1(GPIOC,GPIO_Pin_1);
Init_ADC1(GPIOC,GPIO_Pin_2);//2
Init_ADC1(GPIOC,GPIO_Pin_3);
Init_ADC1(GPIOC,GPIO_Pin_4);//4
Init_ADC1(GPIOC,GPIO_Pin_5);*/
CW0 cw0;cw0.setupDigitalOut();
CW1 cw1;cw1.setupDigitalOut();
CW2 cw2;cw2.setupDigitalOut();
CW3 cw3;cw3.setupDigitalOut();
CW4 cw4;cw4.setupDigitalOut();
CW5 cw5;cw5.setupDigitalOut();
CCW0 ccw0;ccw0.setupDigitalOut();
CCW1 ccw1;ccw1.setupDigitalOut();
CCW2 ccw2;ccw2.setupDigitalOut();
CCW3 ccw3;ccw3.setupDigitalOut();
CCW4 ccw4;ccw4.setupDigitalOut();
CCW5 ccw5;ccw5.setupDigitalOut();
Pwm0 pwm0;pwm0.setupPwmOut(10000,1);
Pwm1 pwm1;pwm1.setupPwmOut(10000,1);
Pwm2 pwm2;pwm2.setupPwmOut(10000,1);
Pwm3 pwm3;pwm3.setupPwmOut(10000,1);
Pwm4 pwm4;pwm4.setupPwmOut(10000,1);
Pwm5 pwm5;pwm5.setupPwmOut(10000,1);
Enc0 enc0;enc0.setup();
Enc1 enc1;enc1.setup();
Enc2 enc2;enc2.setup();
Serial0 serial0;
serial0.setup(115200);
//Blink blink(cw0);blink.setup();
//blink.time(500);
Blink blink0(led0);blink0.setup();blink0.time(150);
Blink blink1(led1);blink1.setup();blink1.time(300);
Blink blink2(led2);blink2.setup();blink2.time(450);
Blink blink3(led3);blink3.setup();blink3.time(600);
//Blink blink4(a4);blink4.setup();blink4.time(2000);
float duty=0;
int flag=0;
int time=0;
int led=0;
while(1){
blink0.cycle();
blink1.cycle();
blink2.cycle();
blink3.cycle();
//blink4.cycle();
if(millis()-time>=60){
time=millis();
/*if(flag==0){
if(duty>=1){
flag=1;
}
duty+=0.1;
}else{
if(duty<0){
flag=0;
}
duty-=0.1;
}
pwm0.pwmWrite(duty);
pwm1.pwmWrite(duty);
pwm2.pwmWrite(duty);
pwm3.pwmWrite(duty);
pwm4.pwmWrite(duty);
pwm5.pwmWrite(duty);*/
if(!sw0.digitalRead()) Reset_encoder_over_under_flow();
/*serial0.printf("a0:%d, ",a0.analogRead());
serial0.printf("a1:%d, ",a1.analogRead());
serial0.printf("a2:%d, ",a2.analogRead());
serial0.printf("a3:%d, ",a3.analogRead());
serial0.printf("a4:%d\n",a4.analogRead());*/
serial0.printf("a0:%.2f",get_ADC1_value(GPIOC,GPIO_Pin_5));
serial0.printf("a1:%.2f",get_ADC1_value(GPIOC,GPIO_Pin_1));
serial0.printf("a2:%.2f",get_ADC1_value(GPIOC,GPIO_Pin_2));
serial0.printf("a3:%.2f",get_ADC1_value(GPIOC,GPIO_Pin_3));
serial0.printf("a4:%.2f\n",get_ADC1_value(GPIOC,GPIO_Pin_4));
//serial0.printf("s0:%d, s1:%d, s2:%d, s3:%d ",sw0.digitalRead(),sw1.digitalRead(),sw2.digitalRead(),sw3.digitalRead());
//serial0.printf("e0:%d, e1:%d, e2:%d\n\r",enc0.count(),enc1.count(),enc2.count());
}
/*if(millis()-led>=150){
led=millis();
cw0.digitalToggle();ccw0.digitalToggle();
cw1.digitalToggle();ccw1.digitalToggle();
cw2.digitalToggle();ccw2.digitalToggle();
cw3.digitalToggle();ccw3.digitalToggle();
cw4.digitalToggle();ccw4.digitalToggle();
cw5.digitalToggle();ccw5.digitalToggle();
}*/
}
return 0;
}
int main(void)
{
Can0 can;
can.setup();
Buzzer buzzer;
Led0 led0;
Led1 led1;
Led2 led2;
Led3 led3;
A1 limBack;
A3 limFlont;
Serial1 serial1;
led0.setupDigitalOut();
led1.setupDigitalOut();
led2.setupDigitalOut();
led3.setupDigitalOut();
buzzer.setupDigitalOut();
CanEncoder canEncoder0(can,0,5);
CanEncoder canEncoder1(can,1,5);
CanEncoder canEncoder2(can,2,5);
canEncoder0.setup();
canEncoder1.setup();
canEncoder2.setup();
Motor motor;
Connection control;
Testmotor t;
R1350n r1350n(serial1);
r1350n.setup();
int timeLed=0;
Position position(canEncoder0,canEncoder1,canEncoder2);
buzzer.digitalHigh();
while(1){
if(control.dispose==0){
led0.digitalWrite(1);
}
else if(control.dispose==1){
led1.digitalWrite(1);
}
else if(control.dispose==2){
led2.digitalWrite(1);
}
if(control.member==1){
led3.digitalWrite(1);
}
else if(control.member==2){
if(millis()-timeLed>=500){
led3.digitalToggle();
timeLed=millis();
}
}
if(millis()-control.period>=cycle){
control.period=millis();
if(control.sw==0&&millis()-control.time>1000){
control.switch0();
}
if(control.sw==1){
position.radian(r1350n.angle());
position.selfPosition();
control.xy(position.integralx,position.integraly);
if(control.spinNumber[control.member][control.dispose][control.point]==1){
motor.motorControl(control.devietionX,control.devietionY);
motor.degreeLock(position.rad);
// motor.testmotor();
motor.last();
motor.dutyCleanUp();
if(control.point<5){
control.coordinatePoint(motor.distance);
}
}
else if(control.spinNumber[control.member][control.dispose][control.point]==0){
motor.angle(position.degree,position.degree);
control.spinControl(position.degree);
}
if(control.actionNumber[control.member][control.dispose][control.point]==1){
if(control.armpwm[control.member][control.dispose][control.point]!=0){
control.armTime();
}
control.arm();
motor.armMotor(control.armpwmC,control.armcwC,control.armccwC);
}
control.indication(position.encf[0],position.encf[1],position.encf[2],position.degree,position.integralx,position.integraly,r1350n.angle());
}
if(millis()-control.time>1000){
control.switch0();
}
}
}
return 0;
}
