main.cpp

00001 //
00002 // mbed-os rev.171(mbed2, very old, deprecated BUT useful)  (r020904,sige)
00003 // 1--CAN-th2mot-mbed2-01_01  
00004 //    packs the position(0-36000) command to the data_segment and 
00005 //    dispatches the std_data_frame to the HT02(=GI8008) motor thru CAN1.
00006 //    returns the received data_seg from the motor.
00007 //    (the actual HT02 motor would send back the response data frame?)
00008 //
00009 //  th2mot(double th, unsigned int id)    (th=theta_in, id=CANid_in) 
00010 //
00011 // data segment specific to the HT02 motor
00012 // [0]=host_addr(0xff=this_MCU, 0x01=motor_#01),
00013 // [1]=func_code(01=pos.cmd), [2]=[4]=reserved(?)
00014 // [3]=acce=*256[rpm/s],    [5]=target velo.[rpm] (of motor axis wo the gear, maybe)
00015 // [6]=pos.lower byte,      [7]=pos.higher byte (of output axis, maybe)[0-36000]
00016 
00017 #include "mbed.h"
00018 #include "CAN.h"
00019 
00020 // Ticker ticker;
00021 DigitalOut led1(LED1);
00022 DigitalOut led2(LED2);
00023 Serial pc(USBTX, USBRX);
00024 
00025 CAN can1(p30, p29);     // (RX,TX)pins in this order
00026 CAN can2( p9, p10);
00027 
00028 char c_sent=0, c_read=0;   // counters of sent and read
00029 char dseg[8]={0xFF, 0x01, 0x00, 0x64, 0x00, 0x80, 0x00, 0x00};  //data_segment
00030 char dseg[9]={0xff, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00};
00031 //  byte manipu using the union Hoge, char MUST be unsigned.
00032 union Hoge{ unsigned char c[2]; uint16_t ii; };
00033 
00034 void th2mot(double th, unsigned int id)
00035 {
00036     Hoge hoge;
00037     pc.printf("+(th2mot).. \n\r");
00038     //  truncates th*100. to uint16_t, which is then packed to dseg[6][7].
00039     //  e.g. th=12.345[deg], then hoge.ii=1234 (=0x04d2) and dseg[6]=0xd2, dseg[7]=0x04
00040     //  follows the little endian
00041     hoge.ii=(uint16_t)(th*100.);         dseg[6]=hoge.c[0]; dseg[7]=hoge.c[1];
00042 
00043     if( can1.write(CANMessage(1, dseg, 8, CANData, CANStandard)) ) {
00044         c_sent++;   led1 = !led1;
00045         pc.printf(" ++sent thru can1. c_sent=%d dseg[6][7]=0x%02X%02X \n\r", c_sent, dseg[6],dseg[7]);  }
00046     else
00047         pc.printf(" ++failed can1.write sent=%d dseg[6][7]=0x%02X%02X \n\r", c_sent, dseg[6],dseg[7]);
00048 }   //  endof th2mot
00049 
00050 
00051 int main()
00052 {
00053     double th; unsigned int id;
00054     CANMessage msg;      // remote msg? what determines the size(msg)?
00055     can1.frequency(1000000);    can2.frequency(1000000);  // CANbus=1Mbps
00056     th=12.3456;  id=1;
00057     //  9600(default) 115200(not max?)  921600(on STM32F446, benkatz)
00058      pc.baud(921600);
00059      
00060      while(1){
00061        pc.printf("+++(main of th2mot)...  +feed theta(double)[deg][0.-360.]");
00062        pc.scanf("%lf", &th);
00063 
00064        th2mot(th, id);
00065        wait(0.2);             // dummy wait 0.2[s]
00066        if(can1.read(msg)) {   // org=can2
00067           c_read++; led2 = !led2;
00068           pc.printf(" ++can2 c_read=%d, d[0..7]_read=(0x", c_read);
00069           for(int i=0; i<8; i++) pc.printf("%02X ", msg.data[i]);
00070           pc.printf(") \n\r");
00071        }   //  endof can1.read
00072        else
00073           pc.printf(" ++failed can1.read...  c_sent=%d\n\r", c_sent);
00074      }     //  endof while(1)-loop
00075      
00076 }          //  endof main
00077