MS9g_move_sin.cpp

00001 #include "MS9g_move_sin.h"
00002 //class Servo_9g
00003 Servo_9g::Servo_9g(){}
00004 Servo_9g::Servo_9g (PinName Pin){  
00005     Servo = new PwmOut(Pin);
00006     Servo->period_ms(20);
00007 }
00008 
00009 Servo_9g::~Servo_9g(){
00010     delete Servo;
00011 }
00012 //end of class Servo_9g
00013 
00014 //class Servo_9g_Move
00015 Servo_9g_Move::Servo_9g_Move():Servo_9g(){}
00016 Servo_9g_Move::Servo_9g_Move(PinName Pin):Servo_9g(Pin){}
00017 Servo_9g_Move::~Servo_9g_Move(){}
00018 
00019 void Servo_9g_Move::Move (float a){
00020         Servo_9g::Servo->write(a * val); // duty-cycle, perсents 0..1
00021 }
00022     
00023 void Servo_9g_Move::NextSin(){
00024         i += speed_inc;
00025         if (i > 2) i = 0;
00026         f = (sin(i * pi) + 1.0) / 2 + 1; //1..2
00027 }
00028 
00029 void Servo_9g_Move::CalCnMove(){
00030     Move((float)f);
00031     NextSin();
00032 }
00033 
00034 void Servo_9g_Move::StartMoveSin(){
00035     timer.attach(this, &Servo_9g_Move::CalCnMove, val); //sine period, sec
00036 }
00037 //end of class Servo_9g_Move
00038 
00039 //class Servo_9g_CAN
00040 Servo_9g_CAN::Servo_9g_CAN(PinName rd, PinName td) :
00041     Servo_9g_Move(),
00042     can(rd, td)
00043 {
00044     
00045 }
00046 Servo_9g_CAN::Servo_9g_CAN(PinName rd, PinName td, int hz) :
00047     Servo_9g_Move(),
00048     can(rd, td, hz)
00049 {
00050     
00051 }
00052     
00053 Servo_9g_CAN::Servo_9g_CAN(PinName servo_pwm, PinName rd, PinName td) :
00054     Servo_9g_Move(servo_pwm),
00055     can(rd, td)
00056 {
00057    CANMessage msg;
00058 }
00059 
00060 Servo_9g_CAN::Servo_9g_CAN(PinName servo_pwm, PinName rd, PinName td, int hz) :
00061     Servo_9g_Move(servo_pwm),
00062     can(rd, td, hz)
00063 {
00064     CANMessage msg;
00065 }
00066 
00067 Servo_9g_CAN::~Servo_9g_CAN(){
00068     
00069 }
00070 
00071 void Servo_9g_CAN::SendNextSin(){
00072     NextSin();
00073     float send = static_cast<float>(f);
00074     can.write(CANMessage(2, (char*)&send, sizeof(send)));
00075 }
00076 
00077 bool Servo_9g_CAN::ReceiveServo(){
00078     if(can.read(msg)){
00079         if (msg.id == 2){
00080             float rec;
00081             memcpy(&rec, msg.data, sizeof(rec));
00082             Move(rec);
00083             return true;
00084         }
00085     }
00086     return false;
00087 }
00088 void Servo_9g_CAN::SendSin(){
00089     timer.attach(this, &Servo_9g_CAN::SendNextSin, val);
00090     
00091 }
00092 void Servo_9g_CAN::sendPosition(char id, float pos){
00093     can.write(CANMessage(id, (char*)&pos, sizeof(pos)));
00094 }
00095 //end of class Servo_9g_CAN