use for experiment before the demonstration at open-campus
Dependencies: FEP ikarashiMDC omni PID R1370
Fork of omni_sample by
main.cpp
- Committer:
- tknara
- Date:
- 2017-08-05
- Revision:
- 5:dee9310ec990
- Parent:
- 4:f1d2000776c6
- Child:
- 6:259deb365510
File content as of revision 5:dee9310ec990:
#include "mbed.h" #include "omni.h" #include "MotorDriverController.h" #include "pin_config.h" #include "FEP.h" #define DEBUG MDC motor(i2c_sda,i2c_scl); Omni omni(4, 135); FEP fep(PC_10,PC_11,200); DigitalOut leds[4] = {PC_13,PC_14,PC_15,PA_0}; DigitalOut attack[2] = {PA_10,PC_4}; DigitalOut angle[2] = {PB_3,PB_5}; #ifdef DEBUG Serial pc(USBTX,USBRX,115200); #endif void init() { for(int i = 0;i<4;i++) { leds[i] = 0; } } void AllActuatorStop() { #ifdef DEBUG pc.printf("All actuators are stop\n"); #endif omni.stop(); for(int j = 0; j < 4; j++) { motor.write(7,j,omni.getOutput(j)); motor.write(6,j,0); } attack[0]=1; attack[1]=0; angle[0]=0; angle[1]=1; } int main() { char data[10] = { 0 }; int i, error_val = 0, tem[2] = {0}, Button1[7] = { 0 }, Button2[6] = { 0 }; uint8_t fep_temp; double polarVector[2]; double deg = 360; double pwm = 0.0; for(int j = 0; j < 4; j++) { motor.write(7,j,0); } while(1) { fep_temp=fep.read(data,6); if(fep_temp==0) { #ifdef DEBUG pc.printf("Recieve succeeded,%s\r\n",data); #endif error_val = 0; leds[0] = 0; tem[0] = data[4]; tem[1] = data[5]; for(i = 0; i < 7; i++) { Button1[i] = tem[0] % 2; tem[0] /= 2; #ifdef DEBUG pc.printf("%d,",Button1[i]); #endif } pc.printf("\n"); for(i = 0; i < 6; i++) { Button2[i] = tem[1] % 2; tem[1] /= 2; #ifdef DEBUG pc.printf("%d,",Button2[i]); #endif } #ifdef DEBUG pc.printf("\r\n"); #endif } else if(fep_temp==FEP_NO_RESPONSE) { #ifdef DEBUG pc.printf("No response\r\n"); #endif leds[0] = 1; continue; } else { #ifdef DEBUG pc.printf("Recieve failed\r\n"); #endif leds[0] = 1; error_val++; } if(error_val < 4) { #ifdef DEBUG //for(i = 0;i<7;i++) printf("%d\n",Button1[i]); #endif polarVector[1] = 0.6; //移動方向 if(Button1[2] == 0 && Button1[3] == 1 && Button1[4] == 1 && Button1[5] == 1) { deg = 270; leds[1] = 0; }else if(Button1[2] == 1 && Button1[3] == 0 && Button1[4] == 1 && Button1[5] == 1) { deg = 180; leds[1] = 0; }else if(Button1[2] == 1 && Button1[3] == 1 && Button1[4] == 0 && Button1[5] == 1) { deg = 90; leds[1] = 0; }else if(Button1[2] == 1 && Button1[3] == 1 && Button1[4] == 1 && Button1[5] == 0) { deg = 0; leds[1] = 0; }else { deg = 360; polarVector[1] = 0; leds[1] = 1; } polarVector[0] = deg; if(data[2] < 50) { omni.computePolar(polarVector, 0.15); leds[2] = 1; leds[3] = 0; } else if(data[2] > 200) { omni.computePolar(polarVector, -0.15); leds[2] = 0; leds[3] = 1; } else { omni.computePolar(polarVector, 0); leds[2] = 0; leds[3] = 0; } for(int j = 0; j < 4; j++) { motor.write(7,j,omni.getOutput(j)); } //昇降 if(Button1[0] == 1 && Button1[1] == 0) { pwm = 0.9; } else if(Button1[0] == 0 && Button1[1] == 1) { pwm = -0.9; } else { pwm = -0.0; } motor.write(6,0,pwm); //アーム攻撃 if((Button2[0]==1)&&(Button2[1]==0)) { attack[0]=0; attack[1]=1; }else if((Button2[0]==0)&&(Button2[1]==1)){ attack[0]=1; attack[1]=0; }else{ attack[0]=0; attack[1]=0; } //アーム角度 if((Button2[2]==1)&&(Button2[3]==0)) { angle[0]=0; angle[1]=1; }else if((Button2[2]==0)&&Button2[3]==1){ angle[0]=1; angle[1]=0; }else{ angle[0]=0; angle[1]=0; } } else if((fep_temp==FEP_NO_RESPONSE)&&(error_val > 4)){ AllActuatorStop(); } else { AllActuatorStop(); } } }