FM-test
Dependencies: MODSERIAL mbed-rtos mbed
Fork of Master by
main.cpp
- Committer:
- 9uS7
- Date:
- 2014-09-15
- Revision:
- 13:3f0505bbe284
- Parent:
- 12:168338a29373
File content as of revision 13:3f0505bbe284:
//Master mbed Program #include "mbed.h" #include "fm.h" #include "bluetooth.h" #include "control.h" //BT_MASTER or BT_SLAVE #define DEVICE_ROLE BT_MASTER //Serial pc(USBTX, USBRX); // tx, rx AnalogIn mic(p17); //debug DigitalOut led1(LED1); DigitalOut led2(LED2); DigitalOut led4(LED4); void masterLoop(void); int main() { //FM_FREQUENCY is defined in fm.h unsigned int fm_frequency = DEVICE_ROLE==BT_MASTER ? FM_FREQUENCY1 : FM_FREQUENCY2; fmSetup( fm_frequency ); //btSetup(DEVICE_ROLE); //motorSetup(); while(1){ if( DEVICE_ROLE==BT_MASTER ){ //masterLoop(); } fmStop(); wait(2); fmRestart(); wait(2); } } void masterLoop(){ float ir_m,fsr_m; //sensor of master float ir_s,fsr_s; //sensor of slave char function_m; //motor function of master char function_s; //motor function of slave float power_m; //motor power of master float power_s; //motor power of slave float dst; getSensor( &ir_m, &fsr_m ); recieveSensor( &ir_s, &fsr_s ); //距離を一定に保つ /* if( (dst=abs( ir_m+ir_s - center_pos*2 ))<0.1 ){ if( fsr_s-fsr_m>0.2 ){ function_m = MOTOR_OPEN; power_m = 0; function_s = MOTOR_PULL; power_s = dst/2; } else if( fsr_m-fsr_s >0.2 ){ function_m = MOTOR_PULL; power_m = dst/2; function_s = MOTOR_OPEN; power_s = 0; } else{ function_m = MOTOR_OPEN; power_m = 0; function_s = MOTOR_OPEN; power_s = 0; } } else if( ir_m+ir_s - center_pos*2 < 0 ){ function_m = MOTOR_LOOSE; power_m = dst/2; function_s = MOTOR_LOOSE; power_s = dst/2; } else{ function_m = MOTOR_PULL; power_m = dst/2; function_s = MOTOR_PULL; power_s = dst/2; } */ function_m = MOTOR_PULL; power_m = 0.1; function_s = MOTOR_PULL; power_s = 0.1; //Master's motor motor( function_m, power_m ); //slave's motor sendMotor( function_s, power_s ); led4 = ( ir_s>0.5 ? 1 : 0 ); led2 = ( led2 ? 0 : 1 ); wait(0.1); }