main.cpp

00001 #include "mbed.h"
00002 #include "CAN_com.h"
00003 #include "structs.h"
00004 
00005 #define PULSE_MAX 2400  // us
00006 #define PULSE_MIN 600   //us  
00007 #define PERIOD 20       // us
00008 #define POS_MAX 3.14159265359f  // radx
00009 #define POS_MIN 0.0f            // rad
00010 #define M_PI 3.14159265359f  // rad
00011 
00012 #define CAN_ID 4
00013 
00014 CAN can(PA_11,PA_12);
00015 Ticker can_ticker;
00016 Timer timer;
00017 
00018 PwmOut led_blue(PB_4);
00019 PwmOut led_red(PB_5);
00020 
00021 PwmOut servo1(PA_3);
00022 PwmOut servo2(PA_1);
00023 
00024 ControllerStruct controller;
00025 
00026 void moveServo(uint8_t id,double degree);
00027 void readCANData();
00028 
00029 bool received = false;
00030 
00031 int main()
00032 {
00033     can.frequency(1000000);
00034     can.filter(CAN_ID, 0xFFF, CANStandard, 0);
00035     can.attach(&readCANData);
00036 
00037     servo1.period_ms(PERIOD);
00038     servo2.period_ms(PERIOD);
00039     while(1) {
00040         if (received == true){
00041             double gripper_pos = controller.p_des;
00042             int pulse1 = (PULSE_MAX - PULSE_MIN) * gripper_pos / (POS_MAX - POS_MIN) + PULSE_MIN;
00043             int pulse2 = (PULSE_MAX - PULSE_MIN) * (M_PI - gripper_pos) / (POS_MAX - POS_MIN) + PULSE_MIN;
00044             servo1.pulsewidth_us(pulse1);
00045             servo2.pulsewidth_us(pulse2);
00046             wait(0.01);
00047         }
00048     }
00049 }
00050 
00051 void readCANData()
00052 {
00053     CANMessage can_msg;
00054     if(can.read(can_msg)) {
00055         if(can_msg.id==CAN_ID) {
00056             unpack_cmd(can_msg,&controller);
00057             received = true;
00058         }
00059     }
00060 }