Abe Takumi / Mbed 2 deprecated EM_Mission

For Cansat mission mbed

Dependencies:   MU2Class RS405cb mbed myCAN_logger

main.cpp@4:934b39bd5c2c, 2013-08-02 (annotated)

Committer:
YSB
Date:
Fri Aug 02 10:51:03 2013 +0000
Revision:
4:934b39bd5c2c
Parent:
2:5871a792937b
Child:
5:e85af85e4985
fvgbnj

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 1:f19466574b75 4 #include "myCAN.h"
YSB 1:f19466574b75 5 #include "IDDATA.h"
YSB 0:7857b4c95c75 6
YSB 0:7857b4c95c75 7 //for debug
YSB 0:7857b4c95c75 8 Serial pc(USBTX,USBRX);
YSB 0:7857b4c95c75 9 DigitalOut led2(LED2);
YSB 0:7857b4c95c75 10 //for communication
YSB 1:f19466574b75 11 myCAN can(p30, p29);
YSB 0:7857b4c95c75 12 MU2Class MyMu2(p9,p10);
YSB 0:7857b4c95c75 13 DigitalOut MU2_permit(p11);
YSB 1:f19466574b75 14 Ticker Send_Call;
YSB 0:7857b4c95c75 15 //for drive actuator
YSB 0:7857b4c95c75 16 RS405cb servo(p13,p14,p18);//TX,RX,PERMIT
YSB 0:7857b4c95c75 17 //for fusing PWM
YSB 0:7857b4c95c75 18 PwmOut nikuromu1(p22);
YSB 0:7857b4c95c75 19 PwmOut nikuromu2(p23);
YSB 0:7857b4c95c75 20 PwmOut nikuromu3(p24);
YSB 0:7857b4c95c75 21 //for Flight confirmation
YSB 0:7857b4c95c75 22 DigitalIn flightpin(p26);
YSB 1:f19466574b75 23
YSB 1:f19466574b75 24 //for pc
YSB 1:f19466574b75 25 #define PC_BAUD 9600
YSB 1:f19466574b75 26 #define COMMUNICATION_RATE 0.3
YSB 1:f19466574b75 27 void data_send(void);
YSB 1:f19466574b75 28
YSB 0:7857b4c95c75 29 //for PWM
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 1:f19466574b75 42 //for pc
YSB 1:f19466574b75 43 pc.baud(PC_BAUD);
YSB 1:f19466574b75 44 Send_Call.attach(&data_send,COMMUNICATION_RATE);
YSB 1:f19466574b75 45
YSB 0:7857b4c95c75 46 //correspondence off
YSB 0:7857b4c95c75 47 MU2_permit = 1;//off
YSB 0:7857b4c95c75 48
YSB 0:7857b4c95c75 49 //PWM init
YSB 0:7857b4c95c75 50 nikuromu1.period(N_PERIOD);
YSB 0:7857b4c95c75 51 NIKUROMU1_OFF
YSB 0:7857b4c95c75 52 nikuromu2.period(N_PERIOD);
YSB 0:7857b4c95c75 53 NIKUROMU2_OFF
YSB 0:7857b4c95c75 54 nikuromu3.period(N_PERIOD);
YSB 0:7857b4c95c75 55 NIKUROMU3_OFF
YSB 1:f19466574b75 56
YSB 4:934b39bd5c2c 57 while(1){}
YSB 4:934b39bd5c2c 58
YSB 1:f19466574b75 59 //Sequence
YSB 4:934b39bd5c2c 60 servo.TORQUE_ON(2); //for rotation
YSB 2:5871a792937b 61 servo.Rotate_Servo_Float(2,-150.0);
YSB 0:7857b4c95c75 62
YSB 1:f19466574b75 63 while(flightpin ==1){}
YSB 1:f19466574b75 64
YSB 1:f19466574b75 65 MU2_permit = 0;//on
YSB 1:f19466574b75 66 servo.TORQUE_ON(1);
YSB 1:f19466574b75 67 servo.TORQUE_ON(2);
YSB 1:f19466574b75 68 servo.TORQUE_ON(3);
YSB 4:934b39bd5c2c 69 wait(0.3);//wait until mu2 kidou
YSB 1:f19466574b75 70
YSB 1:f19466574b75 71 MyMu2.send("housyutukennti\r\n");
YSB 1:f19466574b75 72
YSB 1:f19466574b75 73 wait(10);
YSB 1:f19466574b75 74
YSB 1:f19466574b75 75 MyMu2.send("tizyounituita\r\n");
YSB 1:f19466574b75 76
YSB 1:f19466574b75 77
YSB 1:f19466574b75 78 NIKUROMU1_ON
YSB 1:f19466574b75 79 wait(6.0);
YSB 1:f19466574b75 80 NIKUROMU1_OFF
YSB 1:f19466574b75 81
YSB 1:f19466574b75 82 MyMu2.send("tumehiraita\r\n");
YSB 1:f19466574b75 83
YSB 1:f19466574b75 84
YSB 4:934b39bd5c2c 85 wait(20.0);//changed 20 to 5
YSB 4:934b39bd5c2c 86
YSB 2:5871a792937b 87 servo.TORQUE_ON(2);
YSB 4:934b39bd5c2c 88 servo.Rotate_Servo_Float(2,-150.0);
YSB 4:934b39bd5c2c 89 servo.Rotate_Servo_Float(2,-150.0 + 360.0 - can.get_roll() );
YSB 0:7857b4c95c75 90
YSB 2:5871a792937b 91
YSB 1:f19466574b75 92 MyMu2.send("uemuitayo\r\n");
YSB 0:7857b4c95c75 93
YSB 1:f19466574b75 94 wait(5.0);
YSB 4:934b39bd5c2c 95
YSB 1:f19466574b75 96 NIKUROMU2_ON
YSB 1:f19466574b75 97 wait(6.0);
YSB 1:f19466574b75 98 NIKUROMU2_OFF
YSB 1:f19466574b75 99
YSB 1:f19466574b75 100 MyMu2.send("tobirahiraita\r\n");
YSB 0:7857b4c95c75 101
YSB 4:934b39bd5c2c 102 wait(5.0);
YSB 2:5871a792937b 103
YSB 1:f19466574b75 104 NIKUROMU3_ON
YSB 1:f19466574b75 105 wait(6.0);
YSB 1:f19466574b75 106 NIKUROMU3_OFF
YSB 0:7857b4c95c75 107
YSB 1:f19466574b75 108 MyMu2.send("anntenatennkai\r\n");
YSB 0:7857b4c95c75 109
YSB 4:934b39bd5c2c 110 wait(5.0);
YSB 4:934b39bd5c2c 111
YSB 1:f19466574b75 112 MyMu2.send("sukyannhazime\r\n");
YSB 0:7857b4c95c75 113
YSB 1:f19466574b75 114 RHI_Scan();
YSB 0:7857b4c95c75 115 }
YSB 0:7857b4c95c75 116
YSB 0:7857b4c95c75 117 void RHI_Scan(void){
YSB 0:7857b4c95c75 118 servo.TORQUE_ON(1);
YSB 1:f19466574b75 119 servo.TORQUE_ON(3);
YSB 0:7857b4c95c75 120
YSB 0:7857b4c95c75 121 while(1){
YSB 4:934b39bd5c2c 122 for(int j=0;j<100;j++){
YSB 0:7857b4c95c75 123 for(int i=0;i<181;i++){
YSB 0:7857b4c95c75 124 servo.Rotate_Servo_Float(1,j*1.0);
YSB 1:f19466574b75 125 servo.Rotate_Servo_Float(3,90.0-i*1.0);
YSB 0:7857b4c95c75 126 wait(0.01);
YSB 0:7857b4c95c75 127 }
YSB 0:7857b4c95c75 128 for(int i=0;i<181;i++){
YSB 0:7857b4c95c75 129 servo.Rotate_Servo_Float(1,j*1.0);
YSB 1:f19466574b75 130 servo.Rotate_Servo_Float(3,-90.0+i*1.0);
YSB 0:7857b4c95c75 131 wait(0.01);
YSB 0:7857b4c95c75 132 }
YSB 0:7857b4c95c75 133 wait(0.05);
YSB 0:7857b4c95c75 134 }
YSB 0:7857b4c95c75 135 }
YSB 0:7857b4c95c75 136 }
YSB 1:f19466574b75 137
YSB 1:f19466574b75 138 void data_send(void){
YSB 4:934b39bd5c2c 139 pc.printf("%s,%s,%s,%d,%d\r\n",can.get_time(),can.get_latitude(),can.get_longitude(),can.get_NoS(),can.get_roll());
YSB 1:f19466574b75 140
YSB 1:f19466574b75 141 char mu2data[100];
YSB 1:f19466574b75 142 mu2data[0] = *(can.get_time()+0);
YSB 1:f19466574b75 143 mu2data[1] = *(can.get_time()+1);
YSB 1:f19466574b75 144 mu2data[2] = *(can.get_time()+2);
YSB 1:f19466574b75 145 mu2data[3] = *(can.get_time()+3);
YSB 1:f19466574b75 146 mu2data[4] = *(can.get_time()+4);
YSB 1:f19466574b75 147 mu2data[5] = *(can.get_time()+5);
YSB 1:f19466574b75 148 mu2data[6] = *(can.get_time()+6);
YSB 1:f19466574b75 149 mu2data[7] = *(can.get_time()+7);
YSB 1:f19466574b75 150
YSB 1:f19466574b75 151 mu2data[8]=',';
YSB 1:f19466574b75 152
YSB 1:f19466574b75 153 mu2data[9] = *(can.get_latitude()+0);
YSB 1:f19466574b75 154 mu2data[10] = *(can.get_latitude()+1);
YSB 1:f19466574b75 155 mu2data[11] = *(can.get_latitude()+2);
YSB 1:f19466574b75 156 mu2data[12] = *(can.get_latitude()+3);
YSB 1:f19466574b75 157 mu2data[13] = *(can.get_latitude()+4);
YSB 1:f19466574b75 158 mu2data[14] = *(can.get_latitude()+5);
YSB 1:f19466574b75 159 mu2data[15] = *(can.get_latitude()+6);
YSB 1:f19466574b75 160 mu2data[16] = *(can.get_latitude()+7);
YSB 1:f19466574b75 161 mu2data[17] = *(can.get_latitude()+8);
YSB 1:f19466574b75 162
YSB 1:f19466574b75 163 mu2data[18]=',';
YSB 1:f19466574b75 164
YSB 1:f19466574b75 165 mu2data[19] = *(can.get_longitude()+0);
YSB 1:f19466574b75 166 mu2data[20] = *(can.get_longitude()+1);
YSB 1:f19466574b75 167 mu2data[21] = *(can.get_longitude()+2);
YSB 1:f19466574b75 168 mu2data[22] = *(can.get_longitude()+3);
YSB 1:f19466574b75 169 mu2data[23] = *(can.get_longitude()+4);
YSB 1:f19466574b75 170 mu2data[24] = *(can.get_longitude()+5);
YSB 1:f19466574b75 171 mu2data[25] = *(can.get_longitude()+6);
YSB 1:f19466574b75 172 mu2data[26] = *(can.get_longitude()+7);
YSB 1:f19466574b75 173 mu2data[27] = *(can.get_longitude()+8);
YSB 1:f19466574b75 174 mu2data[28] = *(can.get_longitude()+9);
YSB 1:f19466574b75 175
YSB 1:f19466574b75 176 mu2data[29] = ',';
YSB 1:f19466574b75 177
YSB 1:f19466574b75 178 mu2data[30] = ((can.get_NoS())/10)+0x30;
YSB 1:f19466574b75 179 mu2data[31] = (can.get_NoS()%10)+0x30;
YSB 1:f19466574b75 180
YSB 1:f19466574b75 181 mu2data[32]='\r';
YSB 1:f19466574b75 182 mu2data[33]='\n';
YSB 1:f19466574b75 183 mu2data[34]='\0';
YSB 1:f19466574b75 184
YSB 1:f19466574b75 185 MyMu2.send(mu2data);
YSB 1:f19466574b75 186 }