File content as of revision 1:3befbf3f2f2b:

#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 = 180;
    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("deg  :%lf\n",deg);
        printf("moter:%lf\n",motor);
        printf("deg/s:%f\n",((gyro.read()* 3.3)-1.42)/(0.67/1000));
        //printf("time:%f\n",timec);
    }
}