KPC / Mbed 2 deprecated toritu_sinsi

toritsusinsi

Dependencies:   mbed

main.cpp@1:3befbf3f2f2b, 2019-09-19 (annotated)

Committer:
syosyo
Date:
Thu Sep 19 16:28:07 2019 +0000
Revision:
1:3befbf3f2f2b
Parent:
0:890eb2d5097f 
totirusinsi

Who changed what in which revision?

UserRevisionLine numberNew contents of line
syosyo 0:890eb2d5097f 1 #include "mbed.h"
syosyo 0:890eb2d5097f 2
syosyo 0:890eb2d5097f 3 BusOut lmotor(PA_0,PA_1);
syosyo 0:890eb2d5097f 4 BusOut rmotor(PB_4,PB_5);
syosyo 0:890eb2d5097f 5
syosyo 0:890eb2d5097f 6 DigitalOut test_led(PA_7);
syosyo 0:890eb2d5097f 7
syosyo 0:890eb2d5097f 8 AnalogOut rvset(PA_5);
syosyo 0:890eb2d5097f 9 AnalogOut lvset(PA_4);
syosyo 0:890eb2d5097f 10
syosyo 0:890eb2d5097f 11 AnalogIn gyro(PB_1);
syosyo 0:890eb2d5097f 12
syosyo 0:890eb2d5097f 13 Ticker count_time;
syosyo 0:890eb2d5097f 14
syosyo 0:890eb2d5097f 15 float timec = 0;
syosyo 0:890eb2d5097f 16 const float Kp = 100;
syosyo 0:890eb2d5097f 17 const float Kd = 0;
syosyo 0:890eb2d5097f 18 uint16_t motor = 0;
syosyo 0:890eb2d5097f 19 double deg = 0;
syosyo 0:890eb2d5097f 20 double bdeg = 0;
syosyo 0:890eb2d5097f 21 double sensor;
syosyo 0:890eb2d5097f 22 float unti;
syosyo 0:890eb2d5097f 23 uint16_t vset = 0;
syosyo 0:890eb2d5097f 24
syosyo 0:890eb2d5097f 25
syosyo 0:890eb2d5097f 26 void isr(){
syosyo 0:890eb2d5097f 27 timec += 0.1;
syosyo 1:3befbf3f2f2b 28 //sensor = (double)((gyro.read() * 3.3)-1.35) / (0.67 * 1000.0);
syosyo 1:3befbf3f2f2b 29 sensor = 180;
syosyo 0:890eb2d5097f 30 bdeg = sensor;
syosyo 1:3befbf3f2f2b 31 deg += (double)(sensor * 0.1);
syosyo 0:890eb2d5097f 32
syosyo 0:890eb2d5097f 33 motor = 10 * (0 - deg) - Kd * sensor;
syosyo 0:890eb2d5097f 34 if(motor < 0){
syosyo 0:890eb2d5097f 35 lmotor = 0b10;
syosyo 0:890eb2d5097f 36 rmotor = 0b10;
syosyo 0:890eb2d5097f 37 }
syosyo 0:890eb2d5097f 38 else if(motor > 0){
syosyo 0:890eb2d5097f 39 lmotor = 0b01;
syosyo 0:890eb2d5097f 40 rmotor = 0b01;
syosyo 0:890eb2d5097f 41 }
syosyo 0:890eb2d5097f 42 else{
syosyo 0:890eb2d5097f 43 lmotor = 0b00;
syosyo 0:890eb2d5097f 44 rmotor = 0b00;
syosyo 0:890eb2d5097f 45 }
syosyo 0:890eb2d5097f 46
syosyo 0:890eb2d5097f 47 }
syosyo 0:890eb2d5097f 48
syosyo 0:890eb2d5097f 49 void speed(void){
syosyo 0:890eb2d5097f 50 uint16_t num = motor / 5000;
syosyo 0:890eb2d5097f 51 rvset.write(num);
syosyo 0:890eb2d5097f 52 lvset.write(num);
syosyo 0:890eb2d5097f 53 }
syosyo 0:890eb2d5097f 54
syosyo 0:890eb2d5097f 55 int main(){
syosyo 0:890eb2d5097f 56 wait(3);
syosyo 0:890eb2d5097f 57 count_time.attach(&isr, 0.1);
syosyo 0:890eb2d5097f 58 for(;;){
syosyo 0:890eb2d5097f 59 unti = sensor;
syosyo 0:890eb2d5097f 60 speed();
syosyo 0:890eb2d5097f 61 wait(0.1);
syosyo 0:890eb2d5097f 62 test_led = 1;
syosyo 1:3befbf3f2f2b 63 printf("deg :%lf\n",deg);
syosyo 1:3befbf3f2f2b 64 printf("moter:%lf\n",motor);
syosyo 1:3befbf3f2f2b 65 printf("deg/s:%f\n",((gyro.read()* 3.3)-1.42)/(0.67/1000));
syosyo 1:3befbf3f2f2b 66 //printf("time:%f\n",timec);
syosyo 0:890eb2d5097f 67 }
syosyo 0:890eb2d5097f 68 }