main.cpp

00001 #include "mbed.h"
00002 #include "rtos.h"
00003 #include "pin.h"
00004 #include "angle.h"
00005 #include "can.h"
00006 
00007 Serial pc(USBTX, USBRX);
00008 IRsensor sharp(p17);
00009 CANbus can(p30,p29);
00010 
00011 DigitalOut led(LED1);
00012 //CANbus can(p30,p29);
00013 
00014 Mutex mutex1;
00015 
00016 float curr_req = 0;
00017 float K[3] = {17.0, 3.6, -0.14};
00018 float Mz = 0;
00019 float Mm = 0;
00020 extern float vel_temp = 286331.153;
00021 float phi;      //uhel micku na kole
00022 float omega;    //uhlova rychlost micku
00023 float phi_old;  //starsi hodnota uhlu micku
00024 
00025 int jj = 0;
00026 
00027 
00028 //vlakno pro snimani uhlu micku
00029 void sensor_thread(void const *args) {
00030     while(true) {
00031         mutex1.lock();
00032         phi = sharp.get_angle();
00033         omega = (phi - phi_old)/0.040;
00034         phi_old = phi;
00035         //pc.printf("%2.3f\n",phi);
00036         mutex1.unlock();
00037         Thread::wait(40);
00038     }
00039 }
00040 
00041 //vlakno pro vysilani synchronizace
00042 void sync_thread(void const *args) {
00043     while(true) {
00044         can.servo_sync();
00045         Thread::wait(5);
00046     }
00047 }
00048 
00049 
00050     
00051 int main() {
00052     pc.baud(19200);
00053     pc.format(8,SerialBase::None,1);
00054 
00055     Thread thread_sens(sensor_thread);
00056     Thread thread_sync(sync_thread);
00057     i = 20;
00058     //can.can_init();
00059     can.enable_servo();
00060     can.set_mode();
00061     can.set_current(0);
00062     
00063     while(phi < 0.3 && (rSpeed < 25 || rSpeed > -25) && (curr_req < 15.0 || curr_req > -15.0)){
00064         can.actual_speed();
00065         Mz = (phi*K[0] +omega*K[1] + rSpeed*K[2]);
00066         Mm = Mz/7.7;
00067         curr_req = Mm/0.3;
00068         printf("rSpeed = %2.2f phi = %2.2f comega= %2.2f Mz=%2.2f Mm = %2.2f curr = %2.2f req_curr = %2.2f\n",rSpeed, phi,omega, Mz, Mm,current,curr_req);
00069         can.set_current(curr_req);
00070         //printf("phi = %2.2f\n",phi);
00071         wait(0.04);
00072     }
00073     can.disable_servo();
00074 }