Abe Takumi / Mbed 2 deprecated EM_Mission

For Cansat mission mbed

Dependencies:   MU2Class RS405cb mbed myCAN_logger

main.cpp@0:7857b4c95c75, 2013-07-13 (annotated)

Committer:
YSB
Date:
Sat Jul 13 17:17:40 2013 +0000
Revision:
0:7857b4c95c75
Child:
1:f19466574b75
First Publish

Who changed what in which revision?

UserRevisionLine numberNew contents of line
YSB 0:7857b4c95c75 1 #include "mbed.h"
YSB 0:7857b4c95c75 2 #include "RS405cb.h"
YSB 0:7857b4c95c75 3 #include "MU2Class.h"
YSB 0:7857b4c95c75 4
YSB 0:7857b4c95c75 5 //for debug
YSB 0:7857b4c95c75 6 Serial pc(USBTX,USBRX);
YSB 0:7857b4c95c75 7 DigitalOut led2(LED2);
YSB 0:7857b4c95c75 8 //for communication
YSB 0:7857b4c95c75 9 CAN can(p30, p29);
YSB 0:7857b4c95c75 10 MU2Class MyMu2(p9,p10);
YSB 0:7857b4c95c75 11 DigitalOut MU2_permit(p11);
YSB 0:7857b4c95c75 12 //for drive actuator
YSB 0:7857b4c95c75 13 //PwmOut motor1(p21);
YSB 0:7857b4c95c75 14 RS405cb servo(p13,p14,p18);//TX,RX,PERMIT
YSB 0:7857b4c95c75 15 //for fusing PWM
YSB 0:7857b4c95c75 16 PwmOut nikuromu1(p22);
YSB 0:7857b4c95c75 17 PwmOut nikuromu2(p23);
YSB 0:7857b4c95c75 18 PwmOut nikuromu3(p24);
YSB 0:7857b4c95c75 19 //for Flight confirmation
YSB 0:7857b4c95c75 20 DigitalIn flightpin(p26);
YSB 0:7857b4c95c75 21
YSB 0:7857b4c95c75 22 //for CAN
YSB 0:7857b4c95c75 23 #define CAN_BAUD 4000000
YSB 0:7857b4c95c75 24 int id;
YSB 0:7857b4c95c75 25 char smsg[8];
YSB 0:7857b4c95c75 26
YSB 0:7857b4c95c75 27 //for PWM
YSB 0:7857b4c95c75 28 #define M_PERIOD 0.01
YSB 0:7857b4c95c75 29 #define MOTOR_OFF motor1.pulsewidth(0.0);
YSB 0:7857b4c95c75 30 #define N_PERIOD 0.01
YSB 0:7857b4c95c75 31 #define NIKUROMU1_OFF nikuromu1.pulsewidth(0.0);
YSB 0:7857b4c95c75 32 #define NIKUROMU2_OFF nikuromu2.pulsewidth(0.0);
YSB 0:7857b4c95c75 33 #define NIKUROMU3_OFF nikuromu3.pulsewidth(0.0);
YSB 0:7857b4c95c75 34 #define NIKUROMU1_ON nikuromu1.pulsewidth(0.003);
YSB 0:7857b4c95c75 35 #define NIKUROMU2_ON nikuromu2.pulsewidth(0.003);
YSB 0:7857b4c95c75 36 #define NIKUROMU3_ON nikuromu3.pulsewidth(0.003);
YSB 0:7857b4c95c75 37
YSB 0:7857b4c95c75 38 //for servo
YSB 0:7857b4c95c75 39 void RHI_Scan(void);
YSB 0:7857b4c95c75 40
YSB 0:7857b4c95c75 41 int main() {
YSB 0:7857b4c95c75 42 //correspondence off
YSB 0:7857b4c95c75 43 MU2_permit = 1;//off
YSB 0:7857b4c95c75 44
YSB 0:7857b4c95c75 45 //PWM init
YSB 0:7857b4c95c75 46 //motor1.period(M_PERIOD);
YSB 0:7857b4c95c75 47 //MOTOR_OFF
YSB 0:7857b4c95c75 48 nikuromu1.period(N_PERIOD);
YSB 0:7857b4c95c75 49 NIKUROMU1_OFF
YSB 0:7857b4c95c75 50 nikuromu2.period(N_PERIOD);
YSB 0:7857b4c95c75 51 NIKUROMU2_OFF
YSB 0:7857b4c95c75 52 nikuromu3.period(N_PERIOD);
YSB 0:7857b4c95c75 53 NIKUROMU3_OFF
YSB 0:7857b4c95c75 54
YSB 0:7857b4c95c75 55 //for CAN init
YSB 0:7857b4c95c75 56 CANMessage msg;
YSB 0:7857b4c95c75 57 can.frequency(CAN_BAUD);
YSB 0:7857b4c95c75 58
YSB 0:7857b4c95c75 59
YSB 0:7857b4c95c75 60 //for servo test
YSB 0:7857b4c95c75 61 //led2 =1;
YSB 0:7857b4c95c75 62 //RHI_Scan();
YSB 0:7857b4c95c75 63 //servo.Rotate_Servo_Float_Test(3);
YSB 0:7857b4c95c75 64 //servo.ID_CHANGE(1,3);
YSB 0:7857b4c95c75 65 /*
YSB 0:7857b4c95c75 66 float theta;
YSB 0:7857b4c95c75 67 theta=-150.0;
YSB 0:7857b4c95c75 68 servo.TORQUE_ON(3);
YSB 0:7857b4c95c75 69 servo.Rotate_Servo_Float(3,theta);
YSB 0:7857b4c95c75 70 while(1) {
YSB 0:7857b4c95c75 71 if(can.read(msg)) {
YSB 0:7857b4c95c75 72 pc.printf("Message received: %d,%d\n\r", msg.data[0],msg.data[2]);
YSB 0:7857b4c95c75 73 if(msg.data[2]!=1){
YSB 0:7857b4c95c75 74 servo.TORQUE_ON(3);
YSB 0:7857b4c95c75 75 theta += 1.0;
YSB 0:7857b4c95c75 76 servo.Rotate_Servo_Float(3,theta);
YSB 0:7857b4c95c75 77 //wait(0.02);
YSB 0:7857b4c95c75 78 }
YSB 0:7857b4c95c75 79 else{servo.TORQUE_ON(3);}
YSB 0:7857b4c95c75 80 led2 = !led2;
YSB 0:7857b4c95c75 81 }
YSB 0:7857b4c95c75 82 }
YSB 0:7857b4c95c75 83 */
YSB 0:7857b4c95c75 84
YSB 0:7857b4c95c75 85 //Sequence
YSB 0:7857b4c95c75 86
YSB 0:7857b4c95c75 87 while(flightpin ==1){}
YSB 0:7857b4c95c75 88
YSB 0:7857b4c95c75 89 MU2_permit = 0;//on
YSB 0:7857b4c95c75 90
YSB 0:7857b4c95c75 91 wait(5);
YSB 0:7857b4c95c75 92
YSB 0:7857b4c95c75 93
YSB 0:7857b4c95c75 94 id=1;
YSB 0:7857b4c95c75 95 smsg[0]=1;
YSB 0:7857b4c95c75 96 can.write(CANMessage(id,smsg));
YSB 0:7857b4c95c75 97
YSB 0:7857b4c95c75 98 NIKUROMU1_ON
YSB 0:7857b4c95c75 99 wait(6.0);
YSB 0:7857b4c95c75 100 NIKUROMU1_OFF
YSB 0:7857b4c95c75 101
YSB 0:7857b4c95c75 102 wait(10);
YSB 0:7857b4c95c75 103
YSB 0:7857b4c95c75 104 float theta;
YSB 0:7857b4c95c75 105 theta=-150.0;
YSB 0:7857b4c95c75 106 servo.TORQUE_ON(3);
YSB 0:7857b4c95c75 107 servo.Rotate_Servo_Float(3,theta);
YSB 0:7857b4c95c75 108 while(1) {
YSB 0:7857b4c95c75 109 if(can.read(msg)) {
YSB 0:7857b4c95c75 110 pc.printf("Message received:%d,%d,%d\n\r",msg.id, msg.data[0],msg.data[2]);
YSB 0:7857b4c95c75 111 if(msg.data[2]!=1){
YSB 0:7857b4c95c75 112 servo.TORQUE_ON(3);
YSB 0:7857b4c95c75 113 theta += 1.0;
YSB 0:7857b4c95c75 114 servo.Rotate_Servo_Float(3,theta);
YSB 0:7857b4c95c75 115 }
YSB 0:7857b4c95c75 116 else{
YSB 0:7857b4c95c75 117 servo.TORQUE_ON(3);
YSB 0:7857b4c95c75 118 break;
YSB 0:7857b4c95c75 119 }
YSB 0:7857b4c95c75 120 led2 = !led2;
YSB 0:7857b4c95c75 121 }
YSB 0:7857b4c95c75 122 }
YSB 0:7857b4c95c75 123
YSB 0:7857b4c95c75 124
YSB 0:7857b4c95c75 125 wait(5.0);
YSB 0:7857b4c95c75 126 NIKUROMU2_ON
YSB 0:7857b4c95c75 127 wait(6.0);
YSB 0:7857b4c95c75 128 NIKUROMU2_OFF
YSB 0:7857b4c95c75 129
YSB 0:7857b4c95c75 130 wait(5.0);
YSB 0:7857b4c95c75 131 NIKUROMU3_ON
YSB 0:7857b4c95c75 132 wait(6.0);
YSB 0:7857b4c95c75 133 NIKUROMU3_OFF
YSB 0:7857b4c95c75 134
YSB 0:7857b4c95c75 135 RHI_Scan();
YSB 0:7857b4c95c75 136
YSB 0:7857b4c95c75 137 //MyMu2.send("Hello.This is mbed. ");
YSB 0:7857b4c95c75 138 }
YSB 0:7857b4c95c75 139
YSB 0:7857b4c95c75 140 void RHI_Scan(void){
YSB 0:7857b4c95c75 141 servo.TORQUE_ON(1);
YSB 0:7857b4c95c75 142 servo.TORQUE_ON(2);
YSB 0:7857b4c95c75 143
YSB 0:7857b4c95c75 144 while(1){
YSB 0:7857b4c95c75 145 for(int j=0;j<30;j++){
YSB 0:7857b4c95c75 146 for(int i=0;i<181;i++){
YSB 0:7857b4c95c75 147 servo.Rotate_Servo_Float(1,j*1.0);
YSB 0:7857b4c95c75 148 servo.Rotate_Servo_Float(2,90.0-i*1.0);
YSB 0:7857b4c95c75 149 wait(0.01);
YSB 0:7857b4c95c75 150 }
YSB 0:7857b4c95c75 151 for(int i=0;i<181;i++){
YSB 0:7857b4c95c75 152 servo.Rotate_Servo_Float(1,j*1.0);
YSB 0:7857b4c95c75 153 servo.Rotate_Servo_Float(2,-90.0+i*1.0);
YSB 0:7857b4c95c75 154 wait(0.01);
YSB 0:7857b4c95c75 155 }
YSB 0:7857b4c95c75 156 wait(0.05);
YSB 0:7857b4c95c75 157 }
YSB 0:7857b4c95c75 158 }
YSB 0:7857b4c95c75 159 }