toritsusinsi
Dependencies: mbed
main.cpp
- Committer:
- syosyo
- Date:
- 2019-09-19
- Revision:
- 0:890eb2d5097f
- Child:
- 1:3befbf3f2f2b
File content as of revision 0:890eb2d5097f:
#include "mbed.h" BusOut lmotor(PA_0,PA_1); BusOut rmotor(PB_4,PB_5); DigitalOut test_led(PA_7); AnalogOut rvset(PA_5); AnalogOut lvset(PA_4); AnalogIn gyro(PB_1); Ticker count_time; float timec = 0; const float Kp = 100; const float Kd = 0; uint16_t motor = 0; double deg = 0; double bdeg = 0; double sensor; float unti; uint16_t vset = 0; void isr(){ timec += 0.1; sensor = (double)((gyro.read() * 3.3)-1.35) / (0.67 * 1000.0); //sensor = gyro.read() * 3.3; bdeg = sensor; deg += (double)(sensor *0.1); motor = 10 * (0 - deg) - Kd * sensor; if(motor < 0){ lmotor = 0b10; rmotor = 0b10; } else if(motor > 0){ lmotor = 0b01; rmotor = 0b01; } else{ lmotor = 0b00; rmotor = 0b00; } } void speed(void){ uint16_t num = motor / 5000; rvset.write(num); lvset.write(num); } int main(){ wait(3); count_time.attach(&isr, 0.1); for(;;){ unti = sensor; speed(); wait(0.1); test_led = 1; printf("A:%lf\n",deg); printf("B:%lf\n",unti); printf("c:%lf\n",motor); printf("G:%f\n",((gyro.read()* 3.3)-1.42)/(0.67/1000)); printf("time:%f\n",timec); } }